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)
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)
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)
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
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)
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 __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 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()
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 _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
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
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
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
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
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