def __init__(self, context):
        Plugin.__init__(self, context)
        self.setObjectName('RqtGraspViewer')

        self._widget = QWidget()

        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'),
                               'ui', 'RqtGraspViewer.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RqtGraspViewerUi')
        context.add_widget(self._widget)

        main_layout = QHBoxLayout()
        self._default_labels = [
            "obj_id", "object", "grasp_id", "grasp", "quality"
        ]
        self._filemodel = QStandardItemModel(0, 5)
        self._filemodel.setHorizontalHeaderLabels(self._default_labels)

        self._table_view = QTableView()
        self._table_view.setModel(self._filemodel)
        self._table_view.resizeColumnsToContents()

        main_layout.addWidget(self._table_view)
        self._widget.scrollarea.setLayout(main_layout)

        self.init_services()
        self.init_subscribers()

        # self._table_view.clicked.connect(self.on_table_view_clicked)
        self._table_view.selectionModel().selectionChanged.connect(
            self.on_table_view_select)
Example #2
0
 def __init__(self, parent=None):
     QWidget.__init__(self, parent)
     self._signalMapper = QSignalMapper()
     self._signalMapper.mapped[QObject].connect(self._editingFinishedSlot)
     try:
         self.setupUi(self)
     except AttributeError:
         pass
     self._isUpdateRequired = False
     self._connect(self)
Example #3
0
 def __init__(self, parent = None, title = None, withBtn = True,
              nestedItems = True):
     QWidget.__init__(self, parent)
     ContextMenuWidget.__init__(self)
     self.title = TitleHandler.setup(self, title)
     self._nestedItems = nestedItems
     self._setupUi(withBtn)
     self._setupActions()
     self.setupUi()
     QMetaObject.connectSlotsByName(self)
Example #4
0
 def _createButtons(self):
     btnWidget = QWidget(self)
     btnLayout = QHBoxLayout(btnWidget)
     okBtn = QPushButton("add", self)
     okBtn.clicked.connect(self.accept)
     cancelBtn = QPushButton("cancel", self)
     cancelBtn.clicked.connect(self.reject)
     btnLayout.addWidget(okBtn)
     btnLayout.addWidget(cancelBtn)
     btnWidget.setLayout(btnLayout)
     return btnWidget
Example #5
0
 def _createEntries(self):
     entryWidget = QWidget(self)
     entryLayout = QHBoxLayout(entryWidget)
     inputWidgets = (self._createParamBox(), self._createAutoRange(),
                     self._createLower(), self._createUpper(),
                     self._createBins(), self._createXScale(),
                     self._createYWeight())
     self._labels = dict()
     # assumes same ordering of entryWidgets above and Histogram.displayData
     for col, inputWidget in zip(Histogram.displayData, inputWidgets):
         fieldWidget = QWidget(self)  # combines label + input
         fieldLayout = QVBoxLayout(fieldWidget)
         fieldLayout.setContentsMargins(QMargins())
         # create label, text is set in _selectParam()
         self._labels[col] = QLabel(self)
         self._labels[col].setAlignment(Qt.AlignHCenter)
         # stack label + input
         fieldLayout.addWidget(self._labels[col])
         fieldLayout.addWidget(inputWidget)
         fieldWidget.setLayout(fieldLayout)
         # add field to row of inputs
         entryLayout.addWidget(fieldWidget)
     entryWidget.setLayout(entryLayout)
     self.pbox.setCurrentIndex(0)
     self.lentry.selectAll()  # select the first input by default
     return entryWidget
    def __init__(self, context):
        Plugin.__init__(self, context)
        self.setObjectName('RqtGraspViewer')

        self._widget = QWidget()

        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'), 'ui', 'RqtGraspViewer.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RqtGraspViewerUi')
        context.add_widget(self._widget)

        main_layout = QHBoxLayout()
        self._default_labels = ["obj_id", "object", "grasp_id", "grasp", "quality"]
        self._filemodel = QStandardItemModel(0, 5)
        self._filemodel.setHorizontalHeaderLabels(self._default_labels)

        self._table_view = QTableView()
        self._table_view.setModel(self._filemodel)
        self._table_view.resizeColumnsToContents()

        main_layout.addWidget(self._table_view)
        self._widget.scrollarea.setLayout(main_layout)

        self.init_services()
        self.init_subscribers()

        # self._table_view.clicked.connect(self.on_table_view_clicked)
        self._table_view.selectionModel().selectionChanged.connect(self.on_table_view_select)
Example #7
0
    def __init__(self, context):
        super(SrGuiCountingDemo, self).__init__(context)
        
        # Give QObjects reasonable names
        self.setObjectName('SrGuiCountingDemo')

        # Create QWidget
        self._widget = QWidget()
        
        # Get path to UI file which is a sibling of this file
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/sr_counting_demo.ui')
        
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        
        # Give QObjects reasonable names
        self._widget.setObjectName('SrCountingDemoUi')
        
        # Add widget to the user interface
        context.add_widget(self._widget)

        #Setting the initial state of the controller buttons
        self._widget.btn_1.setEnabled(False)
        self._widget.btn_2.setEnabled(False)
        self._widget.btn_3.setEnabled(False)
        self._widget.btn_4.setEnabled(False)
        self._widget.btn_5.setEnabled(False)
 
        # Attaching the button press event to their actions
        self._widget.play.pressed.connect(self.start_demo) 
        self._widget.btn_1.pressed.connect(self.number_one_clicked)
        self._widget.btn_2.pressed.connect(self.number_two_clicked)
        self._widget.btn_3.pressed.connect(self.number_three_clicked)
        self._widget.btn_4.pressed.connect(self.number_four_clicked)
        self._widget.btn_5.pressed.connect(self.number_five_clicked)
Example #8
0
    def __init__(self, parent, calculator, *args):
        super(ModelWidget, self).__init__(parent, None, *args)
        self._calculator = calculator
        self.title = TitleHandler.setup(self, "Model")
        # get all loadable ScatteringModels from model directory
        self._models = FindModels()

        layout = QVBoxLayout(self)
        layout.setObjectName("modelLayout")
        self.setLayout(layout)

        self.modelBox = QComboBox(self)
        self.modelBox.setFixedWidth(FIXEDWIDTH)
        layout.addWidget(self.modelBox)
        self.modelWidget = QWidget(self)
        paramLayout = QVBoxLayout(self.modelWidget)
        self.modelWidget.setLayout(paramLayout)
        layout.addWidget(self.modelWidget)
Example #9
0
    def setupUi(self, *args):
        # called in MainWindowBase.__init__()
        # put the log widget at the bottom
        self.addDockWidget(Qt.BottomDockWidgetArea, self._setupLogWidget())
        # file widget at the top
        self.toolbox = ToolBox(self)
        self._addToolboxItem(self._setupFileWidget())
        self._addToolboxItem(self._setupDataWidget())
        self._addToolboxItem(self._setupOptimWidget())
        self._addToolboxItem(self._setupModelWidget())
        self._addToolboxItem(self._setupStatsWidget())

        # set up central widget of the main window
        self.centralLayout = QVBoxLayout()
        # put buttons in central widget
        self.centralLayout.addWidget(self.toolbox)
        self.centralLayout.addWidget(self._setupStartButton())
        centralWidget = QWidget(self)
        centralWidget.setLayout(self.centralLayout)
        centralWidget.setSizePolicy(QSizePolicy.Preferred, QSizePolicy.Maximum)
        self.setCentralWidget(centralWidget)
        self.onStartupSignal.connect(self.initUi)
        # set program icon, same for Win+Lin
        icopath = "resources/icon/mcsas.ico"
        if isMac():
            icopath = "resources/icon/mcsas.icns"
        icopath = QFileInfo(makeAbsolutePath(icopath)).absoluteFilePath()
        self.setWindowIcon(QIcon(icopath))
    def __init__(self, context):
        super(HipViewerPlugin, self).__init__(context)
        self.setObjectName('HipViewer')

        self._current_dotcode = None

        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hip_viewer.ui')
        loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('HipViewerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))

        self._scene = QGraphicsScene()
        self._widget.graphics_view.setScene(self._scene)

        self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'), -1)
        self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
        self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
        self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
        self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
        self._widget.graph_type_combo_box.setCurrentIndex(0)

        self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))

        self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
        self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
        self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
        self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)

        self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
        self._widget.load_dot_push_button.pressed.connect(self._load_dot)
        self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
        self._widget.save_dot_push_button.pressed.connect(self._save_dot)
        self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
        self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
        self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
        self._widget.save_as_image_push_button.pressed.connect(self._save_image)

        self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self.connect(self, SIGNAL('update_planning_graph'), self.update_planning_graph_synchronous)

        # start the planner in a separate thread
        planning_node = HipViewerNode(ex_callback = self.update_planning_graph)
        planning_node.start()
Example #11
0
    def __init__(self, context):
        super(SrMoveObject, self).__init__(context)
        self.setObjectName('SrMoveObject')

        self._publisher = None
        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/SrMoveObjects.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('SrMoveObjectsUi')
        context.add_widget(self._widget)

        #attaching the button press event to their actions
        self._widget.btnRefreshList.pressed.connect(self.on_refresh_list_clicked_)
        self._widget.btnZero.pressed.connect(self.on_zero_clicked_)
        self._widget.btnOpposite.pressed.connect(self.on_opposite_clicked_)
        self._widget.btn_wrench.pressed.connect(self.on_apply_wrench_clicked_)
        self._widget.btnSetToPos.pressed.connect(self.on_set_to_position_clicked_)
Example #12
0
 def _setupStartButton(self):
     """Set up "Start/Stop" - button."""
     self.startStopBtn = QPushButton()
     self.startStopBtn.setCheckable(True)
     self.startStopBtn.clicked[bool].connect(self.onStartStopClick)
     btnLayout = QHBoxLayout()
     btnLayout.setContentsMargins(0, 0, 0, 0)
     self.startStopBtn.setSizePolicy(QSizePolicy.Maximum,
                                     QSizePolicy.Maximum)
     btnLayout.addWidget(self.startStopBtn)
     btnWidget = QWidget(self)
     btnWidget.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Maximum)
     btnWidget.setLayout(btnLayout)
     return btnWidget
Example #13
0
class ModelWidget(AlgorithmWidget):
    _calculator = None
    _statsWidget = None  # RangeList for (re-)storing histogram settings
    _models = None

    def __init__(self, parent, calculator, *args):
        super(ModelWidget, self).__init__(parent, None, *args)
        self._calculator = calculator
        self.title = TitleHandler.setup(self, "Model")
        # get all loadable ScatteringModels from model directory
        self._models = FindModels()

        layout = QVBoxLayout(self)
        layout.setObjectName("modelLayout")
        self.setLayout(layout)

        self.modelBox = QComboBox(self)
        self.modelBox.setFixedWidth(FIXEDWIDTH)
        layout.addWidget(self.modelBox)
        self.modelWidget = QWidget(self)
        paramLayout = QVBoxLayout(self.modelWidget)
        self.modelWidget.setLayout(paramLayout)
        layout.addWidget(self.modelWidget)

    def setStatsWidget(self, statsWidget):
        """Sets the statistics widget to use for updating ranges."""
        assert (isinstance(statsWidget, AppSettings))
        self._statsWidget = statsWidget

    def onDataSelected(self, dataobj):
        """Gets the data which is currently selected in the UI and rebuilds
        the model selection box based on compatible models."""
        if not isinstance(dataobj, DataObj):
            return
        try:
            self.modelBox.currentIndexChanged[int].disconnect()
        except:
            pass
        self.modelBox.clear()
        # build selection list of available models
        for modelid in self._models:
            cls, dummy = self._models[modelid]
            if cls is None or not issubclass(cls, dataobj.modelType):
                continue
            category = modelid.split('.')[0:-2]
            if category[0] == self._models.rootName:
                del category[0]
            # set up the display name of the model, may contain category
            displayName = " / ".join(category + [
                cls.name(),
            ])
            # store the unique model identifier alongside its title
            self.modelBox.addItem(displayName, modelid)
        self.modelBox.setCurrentIndex(-1)  # select none first
        self.modelBox.currentIndexChanged[int].connect(self._selectModelSlot)
        # trigger signal by switching from none -> 0
        self.modelBox.setCurrentIndex(0)

    def storeSession(self, section=None):
        if self.appSettings is None or self.model is None:
            return
        model = self.model.name()
        self.setRootGroup()
        self.appSettings.beginGroup(self.objectName())
        self.appSettings.setValue("model", model)
        super(ModelWidget, self).storeSession(model)
        self.appSettings.endGroup()
        self._statsWidget.storeSession()

    def restoreSession(self, model=None):
        """Load last known user settings from persistent app settings."""
        if self.appSettings is None:
            return
        if model is None:
            # get the last model used and select it
            self.appSettings.beginGroup(self.objectName())
            model = self.appSettings.value("model")
            self.appSettings.endGroup()
            # calls restoreSession(model) and storeSession()
            # mind the QSettings.group()
            if not isString(model):  # default model if none set
                model = "Sphere"
            self.selectModel(model)
        else:
            self.appSettings.beginGroup(self.objectName())
            super(ModelWidget, self).restoreSession(model)
            self.appSettings.endGroup()
            self._statsWidget.restoreSession()

    def _selectModelSlot(self, key=None):
        # get the model by its unique name in the items data field
        modelid = self.modelBox.itemData(key)
        if modelid not in self._models:
            return
        model, dummy = self._models[modelid]
        if model is None or not issubclass(model, ScatteringModel):
            return
        # store current settings before changing the model
        self.storeSession()
        self._calculator.model = model()  # instantiate the model class
        # remove parameter widgets from layout
        layout = self.modelWidget.layout()
        self.removeWidgets(self.modelWidget)
        # create new parameter widget based on current selection
        for p in self.model.params():
            try:
                widget = self.makeSetting(p, activeBtns=True)
                layout.addWidget(widget)
            except Exception as e:
                DisplayException(e,
                                 fmt=u"An error occurred on building a "
                                 "UI for parameter '{p}' in model '{m}'."
                                 "<p><nobr>{e}</nobr></p>"
                                 "Please see the log for a traceback.".format(
                                     p=p.name(), m=self.model.name(), e="{e}"))
                self.removeWidgets(self.modelWidget)
                self.modelBox.setCurrentIndex(0)
                return
        layout.addStretch()
        # restore user settings for this model
        self.restoreSession(self.model.name())

    def selectModel(self, model):
        """*model*: string containing the name of the model to select.
        Calls _selectModelSlot() via signal."""
        if not isString(model):
            return
        index = 0
        # search the model with the provided name
        for i in range(0, self.modelBox.count()):
            if self.modelBox.itemText(i).lower() == model.lower().strip():
                index = i
                break
        # set the index found or the first one otherwise
        self.modelBox.setCurrentIndex(index)

    @AlgorithmWidget.algorithm.getter
    def algorithm(self):
        if self._calculator is None:
            return None
        return self._calculator.model

    model = algorithm

    def setSphericalSizeRange(self, minVal, maxVal):
        key = "radius"
        # get parameter display order of magnitude:
        param = None
        for p in self.model.params():
            if key in p.name().lower():
                param = p
                break
        if param is None:
            logging.debug("No 'radius'-named parameter found, "
                          "not setting spherical size range!")
            return False  # nothing to do
        keymin, keymax = key + "min", key + "max"
        if self.get(keymin) is not None and self.get(keymax) is not None:
            self.set(keymin, param.toDisplay(minVal))
            self.set(keymax, param.toDisplay(maxVal))
            return True
        return False
class GraspViewerGUI(Plugin):
    """
    a rqtgui plugin for the grasp viewer
    """
    def __init__(self, context):
        Plugin.__init__(self, context)
        self.setObjectName('RqtGraspViewer')

        self._widget = QWidget()

        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'),
                               'ui', 'RqtGraspViewer.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RqtGraspViewerUi')
        context.add_widget(self._widget)

        main_layout = QHBoxLayout()
        self._default_labels = [
            "obj_id", "object", "grasp_id", "grasp", "quality"
        ]
        self._filemodel = QStandardItemModel(0, 5)
        self._filemodel.setHorizontalHeaderLabels(self._default_labels)

        self._table_view = QTableView()
        self._table_view.setModel(self._filemodel)
        self._table_view.resizeColumnsToContents()

        main_layout.addWidget(self._table_view)
        self._widget.scrollarea.setLayout(main_layout)

        self.init_services()
        self.init_subscribers()

        # self._table_view.clicked.connect(self.on_table_view_clicked)
        self._table_view.selectionModel().selectionChanged.connect(
            self.on_table_view_select)
        #self._widget.treeWidget.itemClicked.connect(self.on_posture_clicked)

    def init_services(self):
        """
        Init services
        """

        service_name = '/display_grasp'
        try:
            rospy.wait_for_service(service_name)
            self._grasp_viz_client = rospy.ServiceProxy(
                service_name, DisplayGrasps)
            rospy.loginfo("Found %s", service_name)
        except rospy.ROSException:
            rospy.logerr("%s did not show up. Giving up", service_name)

    def init_subscribers(self):
        """
        Sets up an action client to communicate with the trajectory controller
        """
        self._grasp_sub = rospy.Subscriber("/grasp_manager/result",
                                           GraspPlanningActionResult,
                                           self.graspable_result_cb)

    def graspable_result_cb(self, msg):
        result = msg.result
        if len(result.grasps) > 0:
            fm = self._filemodel
            fm.clear()
            fm.setHorizontalHeaderLabels(self._default_labels)
            self.grasps = result.grasps
            self.populate_table()

    def populate_table(self):
        """
        update table
        """
        fm = self._filemodel
        i = 0
        for j, grasp in enumerate(self.grasps):
            item_id = QStandardItem(str(i))
            item_id.setEditable(False)
            item_obj = QStandardItem("unknown")
            item_obj.setEditable(False)
            idem_grasp_id = QStandardItem(str(j))
            idem_grasp_id.setEditable(False)
            item_grasp = QStandardItem(grasp.id)
            item_grasp.setEditable(False)
            item_grasp_quality = QStandardItem(str(grasp.grasp_quality))
            item_grasp_quality.setEditable(False)
            fm.appendRow([
                item_id, item_obj, idem_grasp_id, item_grasp,
                item_grasp_quality
            ])
        self._table_view.resizeColumnsToContents()
        #fm = self._filemodel[group_name]
        #item_idx = fm.index(idx, 0)
        #fm.setData(item_idx, str(time_from_start))

    def on_table_view_clicked(self, index):

        items = self._table_view.selectedIndexes()
        for it in items:
            print 'selected item index found at %s' % it.row()

    def on_table_view_select(self, selected, deselected):
        fm = self._filemodel
        items = self._table_view.selectedIndexes()
        if len(items) > 0:
            if len(items) > 5:
                rospy.logwarn("More than 5 items selected, \
                              but grasp viewer can only display 5 grasps at the same time"
                              )
            req = DisplayGraspsRequest()
            for it in items:
                row = it.row()
                obj_idx = fm.index(it.row(), 0)
                obj_id = int(fm.data(obj_idx))
                grasp_idx = fm.index(it.row(), 2)
                grasp_id = int(fm.data(grasp_idx))
                req.grasps.append(self.grasps[grasp_id])
                #print "selected item index found at ", obj_id, grasp_id
            self._grasp_viz_client(req)

    #########
    # Default methods for the rqtgui plugins

    def save_settings(self, global_settings, perspective_settings):
        pass

    def restore_settings(self, global_settings, perspective_settings):
        pass
Example #15
0
class SrGuiCountingDemo(Plugin):

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

        # Create QWidget
        self._widget = QWidget()
        
        # Get path to UI file which is a sibling of this file
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/sr_counting_demo.ui')
        
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        
        # Give QObjects reasonable names
        self._widget.setObjectName('SrCountingDemoUi')
        
        # Add widget to the user interface
        context.add_widget(self._widget)

        #Setting the initial state of the controller buttons
        self._widget.btn_1.setEnabled(False)
        self._widget.btn_2.setEnabled(False)
        self._widget.btn_3.setEnabled(False)
        self._widget.btn_4.setEnabled(False)
        self._widget.btn_5.setEnabled(False)
 
        # Attaching the button press event to their actions
        self._widget.play.pressed.connect(self.start_demo) 
        self._widget.btn_1.pressed.connect(self.number_one_clicked)
        self._widget.btn_2.pressed.connect(self.number_two_clicked)
        self._widget.btn_3.pressed.connect(self.number_three_clicked)
        self._widget.btn_4.pressed.connect(self.number_four_clicked)
        self._widget.btn_5.pressed.connect(self.number_five_clicked)
              
    def start_demo(self):
        """
        This function allows to:
  
        1. run the action server node

        2. run the get_joint_state node 

        3. load some parameters
        """
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui.launch")        
            os._exit(0)
        
        self._widget.play.setEnabled(False)
        self._widget.btn_1.setEnabled(True)
        self._widget.btn_2.setEnabled(True)
        self._widget.btn_3.setEnabled(True)
        self._widget.btn_4.setEnabled(True)
        self._widget.btn_5.setEnabled(True) 
        
    def number_one_clicked(self):
        # send goal = 1 to the action server
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=1")
            os._exit(0)

    def number_two_clicked(self):
        # send goal = 2 to the action server
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=2")
            os._exit(0)
        
    def number_three_clicked(self):
        # send goal = 3 to the action server
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=3")
            os._exit(0)
       
    def number_four_clicked(self):
        # send goal = 4 to the action server
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=4")
            os._exit(0)
        
    def number_five_clicked(self):
        # send goal = 5 to the action server
        newpid = os.fork()
        if(newpid > 0):
            os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=5")
            os._exit(0)
        
    def _unregisterPublisher(self):
        pass    
    

    def shutdown_plugin(self):
        pass
class HipViewerPlugin(QObject):

    _deferred_fit_in_view = Signal()

    def __init__(self, context):
        super(HipViewerPlugin, self).__init__(context)
        self.setObjectName('HipViewer')

        self._current_dotcode = None

        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hip_viewer.ui')
        loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('HipViewerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))

        self._scene = QGraphicsScene()
        self._widget.graphics_view.setScene(self._scene)

        self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'), -1)
        self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
        self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
        self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
        self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
        self._widget.graph_type_combo_box.setCurrentIndex(0)

        self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))

        self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
        self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
        self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
        self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)

        self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
        self._widget.load_dot_push_button.pressed.connect(self._load_dot)
        self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
        self._widget.save_dot_push_button.pressed.connect(self._save_dot)
        self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
        self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
        self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
        self._widget.save_as_image_push_button.pressed.connect(self._save_image)

        self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self.connect(self, SIGNAL('update_planning_graph'), self.update_planning_graph_synchronous)

        # start the planner in a separate thread
        planning_node = HipViewerNode(ex_callback = self.update_planning_graph)
        planning_node.start()

    def save_settings(self, global_settings, perspective_settings):
        perspective_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex())
        perspective_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
        perspective_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked())
        perspective_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
        perspective_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())

    def restore_settings(self, global_settings, perspective_settings):
        self._widget.graph_type_combo_box.setCurrentIndex(int(perspective_settings.value('graph_type_combo_box_index', 0)))
        self._widget.filter_line_edit.setText(perspective_settings.value('filter_line_edit_text', ''))
        self._widget.quiet_check_box.setChecked(perspective_settings.value('quiet_check_box_state', True) in [True, 'true'])
        self._widget.auto_fit_graph_check_box.setChecked(perspective_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
        self._widget.highlight_connections_check_box.setChecked(perspective_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])

    def update_planning_graph(self, op, htree):
        self.emit(SIGNAL('update_planning_graph'), op, htree)
        
    def update_planning_graph_synchronous(self, op, htree):
        graph = hip.dot_from_plan_tree(htree)
        graph.layout('dot')
        self._update_graph_view(graph)

    def _update_graph_view(self, graph):
        self._graph  = graph
        self._redraw_graph_view()

    def _generate_tool_tip(self, url):
        if url is not None and ':' in url:
            item_type, item_path = url.split(':', 1)
            if item_type == 'node':
                tool_tip = 'Node:\n  %s' % (item_path)
                service_names = rosservice.get_service_list(node=item_path)
                if service_names:
                    tool_tip += '\nServices:'
                    for service_name in service_names:
                        try:
                            service_type = rosservice.get_service_type(service_name)
                            tool_tip += '\n  %s [%s]' % (service_name, service_type)
                        except rosservice.ROSServiceIOException, e:
                            tool_tip += '\n  %s' % (e)
                return tool_tip
            elif item_type == 'topic':
                topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
                return 'Topic:\n  %s\nType:\n  %s' % (topic_name, topic_type)
        return url
    def __init__(self, context):
        super(HipViewerPlugin, self).__init__(context)
        self.setObjectName('HipViewer')

        self._current_dotcode = None

        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
                               'hip_viewer.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('HipViewerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self._scene = QGraphicsScene()
        self._widget.graphics_view.setScene(self._scene)

        self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'),
                                                     -1)
        self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
        self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
        self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
        self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
        self._widget.graph_type_combo_box.setCurrentIndex(0)

        self._widget.refresh_graph_push_button.setIcon(
            QIcon.fromTheme('view-refresh'))

        self._widget.highlight_connections_check_box.toggled.connect(
            self._redraw_graph_view)
        self._widget.auto_fit_graph_check_box.toggled.connect(
            self._redraw_graph_view)
        self._widget.fit_in_view_push_button.setIcon(
            QIcon.fromTheme('zoom-original'))
        self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)

        self._widget.load_dot_push_button.setIcon(
            QIcon.fromTheme('document-open'))
        self._widget.load_dot_push_button.pressed.connect(self._load_dot)
        self._widget.save_dot_push_button.setIcon(
            QIcon.fromTheme('document-save-as'))
        self._widget.save_dot_push_button.pressed.connect(self._save_dot)
        self._widget.save_as_svg_push_button.setIcon(
            QIcon.fromTheme('document-save-as'))
        self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
        self._widget.save_as_image_push_button.setIcon(
            QIcon.fromTheme('image'))
        self._widget.save_as_image_push_button.pressed.connect(
            self._save_image)

        self._deferred_fit_in_view.connect(self._fit_in_view,
                                           Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self.connect(self, SIGNAL('update_planning_graph'),
                     self.update_planning_graph_synchronous)

        # start the planner in a separate thread
        planning_node = HipViewerNode(ex_callback=self.update_planning_graph)
        planning_node.start()
class HipViewerPlugin(QObject):

    _deferred_fit_in_view = Signal()

    def __init__(self, context):
        super(HipViewerPlugin, self).__init__(context)
        self.setObjectName('HipViewer')

        self._current_dotcode = None

        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
                               'hip_viewer.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('HipViewerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self._scene = QGraphicsScene()
        self._widget.graphics_view.setScene(self._scene)

        self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'),
                                                     -1)
        self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
        self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
        self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
        self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
        self._widget.graph_type_combo_box.setCurrentIndex(0)

        self._widget.refresh_graph_push_button.setIcon(
            QIcon.fromTheme('view-refresh'))

        self._widget.highlight_connections_check_box.toggled.connect(
            self._redraw_graph_view)
        self._widget.auto_fit_graph_check_box.toggled.connect(
            self._redraw_graph_view)
        self._widget.fit_in_view_push_button.setIcon(
            QIcon.fromTheme('zoom-original'))
        self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)

        self._widget.load_dot_push_button.setIcon(
            QIcon.fromTheme('document-open'))
        self._widget.load_dot_push_button.pressed.connect(self._load_dot)
        self._widget.save_dot_push_button.setIcon(
            QIcon.fromTheme('document-save-as'))
        self._widget.save_dot_push_button.pressed.connect(self._save_dot)
        self._widget.save_as_svg_push_button.setIcon(
            QIcon.fromTheme('document-save-as'))
        self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
        self._widget.save_as_image_push_button.setIcon(
            QIcon.fromTheme('image'))
        self._widget.save_as_image_push_button.pressed.connect(
            self._save_image)

        self._deferred_fit_in_view.connect(self._fit_in_view,
                                           Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self.connect(self, SIGNAL('update_planning_graph'),
                     self.update_planning_graph_synchronous)

        # start the planner in a separate thread
        planning_node = HipViewerNode(ex_callback=self.update_planning_graph)
        planning_node.start()

    def save_settings(self, global_settings, perspective_settings):
        perspective_settings.set_value(
            'graph_type_combo_box_index',
            self._widget.graph_type_combo_box.currentIndex())
        perspective_settings.set_value('filter_line_edit_text',
                                       self._widget.filter_line_edit.text())
        perspective_settings.set_value(
            'quiet_check_box_state', self._widget.quiet_check_box.isChecked())
        perspective_settings.set_value(
            'auto_fit_graph_check_box_state',
            self._widget.auto_fit_graph_check_box.isChecked())
        perspective_settings.set_value(
            'highlight_connections_check_box_state',
            self._widget.highlight_connections_check_box.isChecked())

    def restore_settings(self, global_settings, perspective_settings):
        self._widget.graph_type_combo_box.setCurrentIndex(
            int(perspective_settings.value('graph_type_combo_box_index', 0)))
        self._widget.filter_line_edit.setText(
            perspective_settings.value('filter_line_edit_text', ''))
        self._widget.quiet_check_box.setChecked(
            perspective_settings.value('quiet_check_box_state', True) in
            [True, 'true'])
        self._widget.auto_fit_graph_check_box.setChecked(
            perspective_settings.value('auto_fit_graph_check_box_state', True)
            in [True, 'true'])
        self._widget.highlight_connections_check_box.setChecked(
            perspective_settings.value('highlight_connections_check_box_state',
                                       True) in [True, 'true'])

    def update_planning_graph(self, op, htree):
        self.emit(SIGNAL('update_planning_graph'), op, htree)

    def update_planning_graph_synchronous(self, op, htree):
        graph = hip.dot_from_plan_tree(htree)
        graph.layout('dot')
        self._update_graph_view(graph)

    def _update_graph_view(self, graph):
        self._graph = graph
        self._redraw_graph_view()

    def _generate_tool_tip(self, url):
        if url is not None and ':' in url:
            item_type, item_path = url.split(':', 1)
            if item_type == 'node':
                tool_tip = 'Node:\n  %s' % (item_path)
                service_names = rosservice.get_service_list(node=item_path)
                if service_names:
                    tool_tip += '\nServices:'
                    for service_name in service_names:
                        try:
                            service_type = rosservice.get_service_type(
                                service_name)
                            tool_tip += '\n  %s [%s]' % (service_name,
                                                         service_type)
                        except rosservice.ROSServiceIOException, e:
                            tool_tip += '\n  %s' % (e)
                return tool_tip
            elif item_type == 'topic':
                topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
                return 'Topic:\n  %s\nType:\n  %s' % (topic_name, topic_type)
        return url
Example #19
0
    def makeSetting(self, param, activeBtns=False):
        """Creates an input widget for the provided Parameter and configures
        it appropriately.
        """
        if param is None:
            return None
        widget = QWidget(self)
        layout = QHBoxLayout(widget)
        layout.setContentsMargins(0, 0, 0, 0)
        layout.addStretch()

        try:
            suffix = param.suffix()
        except AttributeError:
            suffix = None

        if suffix is None:
            layout.addWidget(self._makeLabel(param.displayName()))
        else:
            layout.addWidget(
                self._makeLabel(u"{} ({})".format(param.displayName(),
                                                  suffix)))

        widget.setLayout(layout)
        if isString(param.__doc__):
            # word wrapping by rich text: https://stackoverflow.com/a/4796057
            txt = "<span>{0}</span>".format(param.__doc__)
            # add description as tooltip if available for parameter
            widget.setToolTip(txt)

        # create scalar value input widget with min/max limits
        minmaxValue, widgets = None, []
        if isinstance(param, ParameterNumerical):
            minmaxValue = param.min(), param.max()
        if isinstance(param, ParameterFloat):
            minmaxValue = param.displayValueRange()
        decimals = None
        if hasattr(param, "decimals"):
            decimals = param.decimals()
        w = self._makeEntry(param.name(),
                            param.dtype,
                            param.displayValue(),
                            decimals=decimals,
                            minmax=minmaxValue,
                            parent=widget)
        try:  # set special value text for lower bound, simple solution
            special = param.displayValues(w.minimum())
            w.setSpecialValueText(special)
        except:
            pass
        w.setFixedWidth(FIXEDWIDTH)
        widgets.insert(old_div(len(widgets), 2), w)

        # Special widget settings for active fitting parameters:
        activeBtns = activeBtns and isinstance(param, FitParameterBase)
        if activeBtns:
            # create input boxes for user specified active fitting range
            # within default upper/lower from class definition
            # minmaxValue = type(param).min(), type(param).max()
            activeRange = {
                "min": param.displayActiveRange()[0],
                "max": param.displayActiveRange()[1]
            }
            for bound in "min", "max":
                w = self._makeEntry(param.name() + bound,
                                    param.dtype,
                                    activeRange[bound],
                                    decimals=decimals,
                                    minmax=minmaxValue,
                                    parent=widget)
                w.setPrefix(bound + ": ")
                w.setFixedWidth(FIXEDWIDTH)
                widgets.append(w)
            # create *active* buttons for FitParameters only
            w = self._makeEntry(param.name() + "active",
                                bool,
                                param.isActive(),
                                widgetType=QPushButton,
                                parent=widget)
            w.setText("Active")
            w.setFixedWidth(FIXEDWIDTH * .5)
            widgets.append(w)

        # add input widgets to the layout
        for w in widgets:
            layout.addWidget(w)
            # store the parameter name
            w.parameterName = param.name()
        # configure UI accordingly (hide/show widgets)
        # no backend update, ui was just built, data is still in sync
        self.updateParam(param, emitBackendUpdated=False)
        return widget
Example #20
0
 def removeWidgets(widget):
     """Removes all widgets from the layout of the given widget."""
     AlgorithmWidget.clearLayout(widget.layout(), QWidget())
class GraspViewerGUI(Plugin):
    """
    a rqtgui plugin for the grasp viewer
    """

    def __init__(self, context):
        Plugin.__init__(self, context)
        self.setObjectName('RqtGraspViewer')

        self._widget = QWidget()

        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'), 'ui', 'RqtGraspViewer.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RqtGraspViewerUi')
        context.add_widget(self._widget)

        main_layout = QHBoxLayout()
        self._default_labels = ["obj_id", "object", "grasp_id", "grasp", "quality"]
        self._filemodel = QStandardItemModel(0, 5)
        self._filemodel.setHorizontalHeaderLabels(self._default_labels)

        self._table_view = QTableView()
        self._table_view.setModel(self._filemodel)
        self._table_view.resizeColumnsToContents()

        main_layout.addWidget(self._table_view)
        self._widget.scrollarea.setLayout(main_layout)

        self.init_services()
        self.init_subscribers()

        # self._table_view.clicked.connect(self.on_table_view_clicked)
        self._table_view.selectionModel().selectionChanged.connect(self.on_table_view_select)
        #self._widget.treeWidget.itemClicked.connect(self.on_posture_clicked)

    def init_services(self):
        """
        Init services
        """

        service_name = '/display_grasp'
        try:
            rospy.wait_for_service(service_name)
            self._grasp_viz_client = rospy.ServiceProxy(service_name, DisplayGrasps)
            rospy.loginfo("Found %s", service_name)
        except rospy.ROSException:
            rospy.logerr("%s did not show up. Giving up", service_name)

    def init_subscribers(self):
        """
        Sets up an action client to communicate with the trajectory controller
        """
        self._grasp_sub = rospy.Subscriber("/grasp_manager/result", GraspPlanningActionResult,
                                           self.graspable_result_cb)

    def graspable_result_cb(self, msg):
        result = msg.result
        if len(result.grasps) > 0:
            fm = self._filemodel
            fm.clear()
            fm.setHorizontalHeaderLabels(self._default_labels)
            self.grasps = result.grasps
            self.populate_table()

    def populate_table(self):
        """
        update table
        """
        fm = self._filemodel
        i = 0
        for j, grasp in enumerate(self.grasps):
            item_id = QStandardItem(str(i))
            item_id.setEditable(False)
            item_obj = QStandardItem("unknown")
            item_obj.setEditable(False)
            idem_grasp_id = QStandardItem(str(j))
            idem_grasp_id.setEditable(False)
            item_grasp = QStandardItem(grasp.id)
            item_grasp.setEditable(False)
            item_grasp_quality = QStandardItem(str(grasp.grasp_quality))
            item_grasp_quality.setEditable(False)
            fm.appendRow([item_id, item_obj, idem_grasp_id, item_grasp, item_grasp_quality])
        self._table_view.resizeColumnsToContents()
        #fm = self._filemodel[group_name]
        #item_idx = fm.index(idx, 0)
        #fm.setData(item_idx, str(time_from_start))

    def on_table_view_clicked(self, index):

        items = self._table_view.selectedIndexes()
        for it in items:
            print 'selected item index found at %s' % it.row()

    def on_table_view_select(self, selected, deselected):
        fm = self._filemodel
        items = self._table_view.selectedIndexes()
        if len(items) > 0:
            if len(items) > 5:
                rospy.logwarn("More than 5 items selected, \
                              but grasp viewer can only display 5 grasps at the same time")
            req = DisplayGraspsRequest()
            for it in items:
                row = it.row()
                obj_idx = fm.index(it.row(), 0)
                obj_id = int(fm.data(obj_idx))
                grasp_idx = fm.index(it.row(), 2)
                grasp_id = int(fm.data(grasp_idx))
                req.grasps.append(self.grasps[grasp_id])
                #print "selected item index found at ", obj_id, grasp_id
            self._grasp_viz_client(req)

    #########
    # Default methods for the rqtgui plugins

    def save_settings(self, global_settings, perspective_settings):
        pass

    def restore_settings(self, global_settings, perspective_settings):
        pass
Example #22
0
class SrMoveObject(Plugin):

    def __init__(self, context):
        super(SrMoveObject, self).__init__(context)
        self.setObjectName('SrMoveObject')

        self._publisher = None
        self._widget = QWidget()

        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/SrMoveObjects.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('SrMoveObjectsUi')
        context.add_widget(self._widget)

        #attaching the button press event to their actions
        self._widget.btnRefreshList.pressed.connect(self.on_refresh_list_clicked_)
        self._widget.btnZero.pressed.connect(self.on_zero_clicked_)
        self._widget.btnOpposite.pressed.connect(self.on_opposite_clicked_)
        self._widget.btn_wrench.pressed.connect(self.on_apply_wrench_clicked_)
        self._widget.btnSetToPos.pressed.connect(self.on_set_to_position_clicked_)

    def on_zero_clicked_(self):
        self._widget.lineEdit.setText("0.0")
        self._widget.lineEdit_2.setText("0.0")
        self._widget.lineEdit_3.setText("0.0")

    def on_opposite_clicked_(self):
        self._widget.lineEdit.setText( str((-1) * float(str(self._widget.lineEdit.text()))) )
        self._widget.lineEdit_2.setText( str((-1) * float(str(self._widget.lineEdit_2.text()))) )
        self._widget.lineEdit_3.setText( str((-1) * float(str(self._widget.lineEdit_3.text()))) )

    def on_set_to_position_clicked_(self):
        success = True
        set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        model_state = ModelState()
        model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0]
        model_state.pose = Pose()
        model_state.twist = Twist()
        model_state.reference_frame = "world"
        
        model_state.pose.position.x = float(str(self._widget.lineEdit_4.text()))
        model_state.pose.position.y = float(str(self._widget.lineEdit_5.text()))
        model_state.pose.position.z = float(str(self._widget.lineEdit_6.text()))

        try:
            resp1 = set_model_state(model_state)
        except rospy.ServiceException:
            success = False
        if success:
            if not resp1.success:
                success = False

        if not success:
            QMessageBox.warning(self._widget, "Warning", "Could not set model state.")

    def on_refresh_list_clicked_(self):
        self._widget.comboBoxObjectList.clear()
        
        success = True
        get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
        object_list = list()
        try:
            resp1 = get_world_properties()
        except rospy.ServiceException:
            success = False
        if success:
            get_model_properties = rospy.ServiceProxy('/gazebo/get_model_properties', GetModelProperties)
            for model in resp1.model_names:
                try:
                    model_properties = get_model_properties(model)
                except rospy.ServiceException:
                    success = False
                if success:
                    for body in model_properties.body_names:
                        object_list.append(model + '::' + body)
        self._widget.comboBoxObjectList.addItems(object_list)
        if not success:
            QMessageBox.warning(self._widget, "Warning", "Could not load object list from Gazebo.")

    def on_apply_wrench_clicked_(self):
        success = True
        apply_body_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench)
        body_name = self._widget.comboBoxObjectList.currentText()
        wrench = Wrench()
        wrench.force.x = float(str(self._widget.lineEdit.text()))
        wrench.force.y = float(str(self._widget.lineEdit_2.text()))
        wrench.force.z = float(str(self._widget.lineEdit_3.text()))

        try:
            resp1 = apply_body_wrench(body_name, "", None, wrench, rospy.Time.from_sec(0), rospy.Duration.from_sec(1.0))
        except rospy.ServiceException:
            success = False
        if success:
            if not resp1.success:
                success = False

        if not success:
            QMessageBox.warning(self._widget, "Warning", "Could not apply wrench to selected object.")



    def _unregisterPublisher(self):
        if self._publisher is not None:
            self._publisher.unregister()
            self._publisher = None

    def shutdown_plugin(self):
        self._unregisterPublisher()

    def save_settings(self, global_settings, perspective_settings):
        pass

    def restore_settings(self, global_settings, perspective_settings):
        pass