def __init__(self, parent=None, current_values=None):
super(BlacklistDialog, self).__init__(parent)
self.setWindowTitle("Blacklist")
vbox = QVBoxLayout()
self.setLayout(vbox)
self._blacklist = Blacklist()
if isinstance(current_values, list):
for val in current_values:
self._blacklist.append(val)
vbox.addWidget(self._blacklist)
controls_layout = QHBoxLayout()
add_button = QPushButton(icon=QIcon.fromTheme('list-add'))
rem_button = QPushButton(icon=QIcon.fromTheme('list-remove'))
ok_button = QPushButton("Ok")
cancel_button = QPushButton("Cancel")
add_button.clicked.connect(self._add_item)
rem_button.clicked.connect(self._remove_item)
ok_button.clicked.connect(self.accept)
cancel_button.clicked.connect(self.reject)
controls_layout.addWidget(add_button)
controls_layout.addWidget(rem_button)
controls_layout.addStretch(0)
controls_layout.addWidget(ok_button)
controls_layout.addWidget(cancel_button)
vbox.addLayout(controls_layout)
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.line_edit = QLineEdit()
layout_.addWidget(self.line_edit)
self.apply_btn = QPushButton("Apply")
self.apply_btn.clicked.connect(self.apply_clicked)
layout_.addWidget(self.apply_btn)
layout.addLayout(layout_)
self.fig = Figure((5, 4), dpi=100)
self.canvas = FigureCanvas(self.fig)
self.axes = self.fig.add_subplot(111)
self.fig.tight_layout()
layout.addWidget(self.canvas)
context.add_widget(self._widget)
self.update_signal.connect(self.update_spectrogram)
self.subscriber_signal.connect(self.update_subscriber)
self.subscriber_signal.emit('spec')
def setup_group_frame(self, group):
layout = QVBoxLayout()
# grid for buttons for named targets
grid = QGridLayout()
grid.setSpacing(1)
self.button_group = QButtonGroup(self)
row = 0
column = 0
named_targets = self.get_named_targets(group)
for target in named_targets:
button = QPushButton(target)
self.button_group.addButton(button)
grid.addWidget(button, row, column)
column += 1
if column >= self.MAX_COLUMNS:
row += 1
column = 0
self.button_group.buttonClicked.connect(self._handle_button_clicked)
# grid for show joint value and move arm buttons/text field
joint_values_grid = QGridLayout()
joint_values_grid.setSpacing(1)
button_show_joint_values = QPushButton('Current Joint Values')
button_show_joint_values.clicked[bool].connect(
self._handle_show_joint_values_clicked)
line_edit = QLineEdit()
self.text_joint_values.append(line_edit)
button_move_to = QPushButton('Move to')
button_move_to.clicked[bool].connect(self._handle_move_to_clicked)
joint_values_grid.addWidget(button_show_joint_values, 0, 1)
joint_values_grid.addWidget(line_edit, 0, 2)
joint_values_grid.addWidget(button_move_to, 0, 3)
layout.addLayout(grid)
line = QFrame()
line.setFrameShape(QFrame.HLine)
line.setFrameShadow(QFrame.Sunken)
layout.addWidget(line)
layout.addLayout(joint_values_grid)
frame = QFrame()
frame.setLayout(layout)
return frame
def __init__(self, parent=None, subscribe=False):
QWidget.__init__(self, parent)
# start widget
vbox = QVBoxLayout()
vbox.setMargin(0)
vbox.setContentsMargins(0, 0, 0, 0)
# add error status text edit
self.error_status_text_box = QErrorStatusTextBox()
self.error_status_text_box_layout = QHBoxLayout()
self.error_status_text_box_layout.addWidget(self.error_status_text_box)
vbox.addLayout(self.error_status_text_box_layout)
# add panel
hbox = QHBoxLayout()
# clear push button
self.execute_command = QPushButton("Clear")
self.execute_command.clicked.connect(self.error_status_text_box.clear)
hbox.addWidget(self.execute_command)
hbox.addStretch()
# hide window checkbox
hide_window_check_box = QCheckBox("Hide")
hide_window_check_box.stateChanged.connect(self.state_changed)
hbox.addWidget(hide_window_check_box)
# end panel
vbox.addLayout(hbox)
# end widget
self.setLayout(vbox)
#self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Minimum)
# subscriber
if subscribe:
self.error_status_sub = rospy.Subscriber("error_status", ErrorStatus, self.error_status_callback)
self.subscribed = subscribe
# connect signal slot internally to prevent crash by subscriber
self.error_status_signal.connect(self.append_error_status)
def __init__(self, parent=None, logger=Logger()):
QWidgetWithLogger.__init__(self, parent, logger)
# start widget
vbox = QVBoxLayout()
vbox.setMargin(0)
vbox.setContentsMargins(0, 0, 0, 0)
# add layout which is dynamically filled
self.parameter_tree_widget = QParameterTreeWidget(logger=self.logger)
vbox.addWidget(self.parameter_tree_widget)
# button panel
vbox_commands = QVBoxLayout()
hbox = QHBoxLayout()
# upload topic
send_parameter_topic_widget = QTopicWidget(self, 'vigir_footstep_planning_msgs/SetParameterSetAction', True)
send_parameter_topic_widget.topic_changed_signal.connect(self._init_upload_paramater_set_client)
hbox.addWidget(send_parameter_topic_widget)
# upload command
self.upload_command = QPushButton("Upload")
self.upload_command.clicked.connect(self.upload_parameters)
hbox.addWidget(self.upload_command)
vbox_commands.addLayout(hbox)
# reload command
self.reload_command = QPushButton("Reload")
self.reload_command.clicked.connect(self.reload_parameters)
vbox_commands.addWidget(self.reload_command)
# add button panel
vbox.addLayout(vbox_commands)
# end widget
self.setLayout(vbox)
# init
self.clear()
send_parameter_topic_widget.emit_topic_name()
def __init__(self, parent=None):
super(VisualizerWidget, self).__init__(parent)
self.setWindowTitle('Graph Profiler Visualizer')
vbox = QVBoxLayout()
self.setLayout(vbox)
toolbar_layout = QHBoxLayout()
refresh_button = QPushButton()
refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
auto_refresh_checkbox = QCheckBox("Auto Refresh")
hide_disconnected_topics = QCheckBox("Hide Disconnected Topics")
topic_blacklist_button = QPushButton("Topic Blacklist")
node_blacklist_button = QPushButton("Node Blacklist")
refresh_button.clicked.connect(self._refresh)
topic_blacklist_button.clicked.connect(self._edit_topic_blacklist)
node_blacklist_button.clicked.connect(self._edit_node_blacklist)
auto_refresh_checkbox.setCheckState(2)
auto_refresh_checkbox.stateChanged.connect(self._autorefresh_changed)
hide_disconnected_topics.setCheckState(2)
hide_disconnected_topics.stateChanged.connect(self._hidedisconnectedtopics_changed)
toolbar_layout.addWidget(refresh_button)
toolbar_layout.addWidget(auto_refresh_checkbox)
toolbar_layout.addStretch(0)
toolbar_layout.addWidget(hide_disconnected_topics)
toolbar_layout.addWidget(topic_blacklist_button)
toolbar_layout.addWidget(node_blacklist_button)
vbox.addLayout(toolbar_layout)
# Initialize the Visualizer
self._view = qt_view.QtView()
self._adapter = rosprofiler_adapter.ROSProfileAdapter(self._view)
self._adapter.set_topic_quiet_list(TOPIC_BLACKLIST)
self._adapter.set_node_quiet_list(NODE_BLACKLIST)
vbox.addWidget(self._view)
class LogicalMarkerPlugin(Plugin):
def __init__(self, context):
super(LogicalMarkerPlugin, self).__init__(context)
# Create an image for the original map. This will never change.
try:
self.map_yaml_file_str = rospy.get_param("~map_file")
self.data_directory = rospy.get_param("~data_directory")
except KeyError:
rospy.logfatal(
"~map_file and ~data_directory need to be set to use the logical marker"
)
return
map_image_location = getImageFileLocation(self.map_yaml_file_str)
map = loadMapFromFile(self.map_yaml_file_str)
locations_file = getLocationsFileLocationFromDataDirectory(
self.data_directory)
doors_file = getDoorsFileLocationFromDataDirectory(self.data_directory)
objects_file = getObjectsFileLocationFromDataDirectory(
self.data_directory)
# Give QObjects reasonable names
self.setObjectName('LogicalMarkerPlugin')
# Create QWidget
self.master_widget = QWidget()
self.master_layout = QVBoxLayout(self.master_widget)
# Main Functions - Doors, Locations, Objects
self.function_layout = QHBoxLayout()
self.master_layout.addLayout(self.function_layout)
self.function_buttons = []
self.current_function = None
for button_text in ['Locations', 'Doors', 'Objects']:
button = QPushButton(button_text, self.master_widget)
button.clicked[bool].connect(self.handle_function_button)
button.setCheckable(True)
self.function_layout.addWidget(button)
self.function_buttons.append(button)
self.function_layout.addStretch(1)
self.master_layout.addWidget(self.get_horizontal_line())
# Subfunction toolbar
self.subfunction_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.subfunction_layout)
self.master_layout.addLayout(self.subfunction_layout)
self.current_subfunction = None
self.master_layout.addWidget(self.get_horizontal_line())
self.image = MapImage(map_image_location, self.master_widget)
self.master_layout.addWidget(self.image)
self.master_layout.addWidget(self.get_horizontal_line())
# Configuration toolbar
self.configuration_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.configuration_layout)
self.master_layout.addLayout(self.configuration_layout)
# Add a stretch at the bottom.
self.master_layout.addStretch(1)
self.master_widget.setObjectName('LogicalMarkerPluginUI')
if context.serial_number() > 1:
self.master_widget.setWindowTitle(
self.master_widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.master_widget)
# Activate the functions
self.functions = {}
self.functions['Locations'] = LocationFunction(
locations_file, map, self.master_widget, self.subfunction_layout,
self.configuration_layout, self.image)
self.functions['Doors'] = DoorFunction(
doors_file, map, self.functions['Locations'], self.master_widget,
self.subfunction_layout, self.configuration_layout, self.image)
self.functions['Objects'] = ObjectFunction(
objects_file, map, self.functions['Locations'], self.master_widget,
self.subfunction_layout, self.configuration_layout, self.image)
def construct_layout(self):
pass
def get_horizontal_line(self):
"""
http://stackoverflow.com/questions/5671354/how-to-programmatically-make-a-horizontal-line-in-qt
"""
hline = QFrame()
hline.setFrameShape(QFrame.HLine)
hline.setFrameShadow(QFrame.Sunken)
return hline
def handle_function_button(self):
source = self.sender()
if source.text() == self.current_function:
source.setChecked(True)
return
# Depress all other buttons.
for button in self.function_buttons:
if button != source:
button.setChecked(False)
if self.current_function is not None:
self.functions[self.current_function].deactivateFunction()
self.current_function = source.text()
# Clear all subfunction buttons.
clearLayoutAndFixHeight(self.subfunction_layout)
if self.current_function is not None:
self.functions[self.current_function].activateFunction()
def shutdown_plugin(self):
modified = False
for function in self.functions:
if self.functions[function].isModified():
modified = True
if modified:
ret = QMessageBox.warning(
self.master_widget, "Save",
"The logical map has been modified.\n"
"Do you want to save your changes?",
QMessageBox.Save | QMessageBox.Discard)
if ret == QMessageBox.Save:
for function in self.functions:
self.functions[function].saveConfiguration()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class QuestionDialogPlugin(Plugin):
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
font_size = rospy.get_param("~font_size", 40)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QHBoxLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.connect(self._widget, SIGNAL("update"), self.update)
self.connect(self._widget, SIGNAL("timeout"), self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self._widget.emit(SIGNAL("update"))
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return QuestionDialogResponse(QuestionDialogRequest.PREEMPTED, "")
if req.timeout != QuestionDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self._widget.emit(SIGNAL("timeout"))
return QuestionDialogResponse(
QuestionDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
req = self.request
self._text_browser.setText(req.message)
if req.type == QuestionDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = QuestionDialogResponse(
QuestionDialogRequest.NO_RESPONSE, "")
self.response_ready = True
elif req.type == QuestionDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
self._button_layout.addWidget(button)
self.buttons.append(button)
elif req.type == QuestionDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = QuestionDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = QuestionDialogResponse(
QuestionDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class MessageBox(QDialog):
NoIcon = 0
Information = 1
Warning = 2
Critical = 3
Question = 4
NoButton = 0
Ok = 1 # An "OK" button defined with the AcceptRole .
Open = 2 # A "Open" button defined with the AcceptRole .
Save = 4 # A "Save" button defined with the AcceptRole .
Cancel = 8 # A "Cancel" button defined with the RejectRole .
Close = 16 # A "Close" button defined with the RejectRole .
Discard = 32 # A "Discard" or "Don't Save" button, depending on the platform, defined with the DestructiveRole .
Apply = 64 # An "Apply" button defined with the ApplyRole .
Reset = 128 # A "Reset" button defined with the ResetRole .
RestoreDefaults = 256 # A "Restore Defaults" button defined with the ResetRole .
Help = 512 # A "Help" button defined with the HelpRole .
SaveAll = 1024 # A "Save All" button defined with the AcceptRole .
Yes = 2048 # A "Yes" button defined with the YesRole .
YesToAll = 4096 # A "Yes to All" button defined with the YesRole .
No = 8192 # A "No" button defined with the NoRole .
NoToAll = 16384 # A "No to All" button defined with the NoRole .
Abort = 32768 # An "Abort" button defined with the RejectRole .
Retry = 65536 # A "Retry" button defined with the AcceptRole .
Ignore = 131072 # An "Ignore" button defined with the AcceptRole .
Avoid = 262144 # An "'Don't show again'" button defined with the HelpRole, returns a default AcceptButton .
def __init__(self,
icon,
title,
text,
detailed_text="",
buttons=Cancel | Ok,
parent=None):
QDialog.__init__(self, parent=parent)
self.setWindowFlags(self.windowFlags() & ~Qt.WindowTitleHint)
self.setWindowFlags(self.windowFlags()
& ~Qt.WindowContextHelpButtonHint
& ~Qt.WindowMinimizeButtonHint)
self.setObjectName('MessageBox')
self._use_checkbox = True
self.text = text
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
self.horizontalLayout = QHBoxLayout()
self.horizontalLayout.setObjectName("horizontalLayout")
self.horizontalLayout.setContentsMargins(1, 1, 1, 1)
# create icon
pixmap = None
if icon == self.NoIcon:
pass
elif icon == self.Question:
pixmap = QPixmap(
QImage(":icons/question.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Information:
pixmap = QPixmap(
QImage(":icons/info.png").scaled(56, 56, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Warning:
pixmap = QPixmap(
QImage(":icons/warning.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Critical:
pixmap = QPixmap(
QImage(":icons/critical.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
spacerItem = QSpacerItem(10, 60, QSizePolicy.Minimum,
QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
self.icon_label = QLabel()
if pixmap is not None:
self.icon_label.setPixmap(pixmap)
self.icon_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
self.horizontalLayout.addWidget(self.icon_label)
spacerItem = QSpacerItem(10, 60, QSizePolicy.Minimum,
QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add message
self.message_label = QLabel(text)
self.message_label.setWordWrap(True)
self.message_label.setScaledContents(True)
self.message_label.setOpenExternalLinks(True)
self.horizontalLayout.addWidget(self.message_label)
self.verticalLayout.addLayout(self.horizontalLayout)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self._accept_button = None
self._reject_button = None
self._buttons = buttons
self._create_buttons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
if detailed_text:
self.btn_show_details = QPushButton(self.tr('Details...'))
self.btn_show_details.setCheckable(True)
self.btn_show_details.setChecked(True)
self.btn_show_details.toggled.connect(self.on_toggled_details)
self.buttonBox.addButton(self.btn_show_details,
QDialogButtonBox.ActionRole)
# create area for detailed text
self.textEdit = textEdit = QTextEdit(self)
textEdit.setObjectName("textEdit")
textEdit.setReadOnly(True)
textEdit.setText(detailed_text)
# textEdit.setVisible(False)
self.verticalLayout.addWidget(self.textEdit)
self.resize(480, self.verticalLayout.totalSizeHint().height())
buttons_in_box = self.buttonBox.buttons()
if buttons_in_box:
self.buttonBox.buttons()[0].setFocus()
def setAcceptButton(self, button):
'''
Sets the button with given ID to accept button if more then one button with AcceptRole was added to this dialog.
Adds the buttton to the box if is not already in.
:param int button: button id
'''
if not button & self._buttons:
self._create_buttons(button)
self._accept_button = button
def setRejectButton(self, button):
'''
Sets the button with given ID to reject button if more then one button with RejectRole was added to this dialog.
Adds the buttton to the box if is not already in.
:param int button: button id
'''
if not button & self._buttons:
self._create_buttons(button)
self._reject_button = button
def on_toggled_details(self, checked):
if checked:
self.verticalLayout.addWidget(self.textEdit)
else:
self.verticalLayout.removeWidget(self.textEdit)
self.textEdit.setVisible(checked)
if not self.isMaximized():
self.setMinimumSize(self.verticalLayout.totalMinimumSize())
self.resize(self._current_size.width(),
self.verticalLayout.totalSizeHint().height())
@staticmethod
def about(parent, title, text, detailed_text='', buttons=Close):
box = MessageBox(MessageBox.Information,
title,
text,
detailed_text=detailed_text,
buttons=buttons,
parent=parent)
if MessageBox.Yes & buttons:
box.setAcceptButton(MessageBox.Yes)
if MessageBox.Cancel & buttons:
box.setRejectButton(MessageBox.Cancel)
elif MessageBox.No & buttons:
box.setRejectButton(MessageBox.No)
return box.exec_()
@staticmethod
def information(parent, title, text, detailed_text='', buttons=Close):
box = MessageBox(MessageBox.Information,
title,
text,
detailed_text=detailed_text,
buttons=buttons,
parent=parent)
if MessageBox.Yes & buttons:
box.setAcceptButton(MessageBox.Yes)
if MessageBox.Cancel & buttons:
box.setRejectButton(MessageBox.Cancel)
elif MessageBox.No & buttons:
box.setRejectButton(MessageBox.No)
return box.exec_()
@staticmethod
def question(parent,
title,
text,
detailed_text='',
buttons=Yes | No | Cancel):
box = MessageBox(MessageBox.Question,
title,
text,
detailed_text=detailed_text,
buttons=buttons,
parent=parent)
if MessageBox.Yes & buttons:
box.setAcceptButton(MessageBox.Yes)
if MessageBox.Cancel & buttons:
box.setRejectButton(MessageBox.Cancel)
elif MessageBox.No & buttons:
box.setRejectButton(MessageBox.No)
return box.exec_()
@staticmethod
def warning(parent, title, text, detailed_text='', buttons=Ok):
box = MessageBox(MessageBox.Warning,
title,
text,
detailed_text=detailed_text,
buttons=buttons,
parent=parent)
if MessageBox.Yes & buttons:
box.setAcceptButton(MessageBox.Yes)
if MessageBox.Cancel & buttons:
box.setRejectButton(MessageBox.Cancel)
elif MessageBox.No & buttons:
box.setRejectButton(MessageBox.No)
return box.exec_()
@staticmethod
def critical(parent, title, text, detailed_text='', buttons=Ok):
box = MessageBox(MessageBox.Critical,
title,
text,
detailed_text=detailed_text,
buttons=buttons,
parent=parent)
if MessageBox.Yes & buttons:
box.setAcceptButton(MessageBox.Yes)
if MessageBox.Cancel & buttons:
box.setRejectButton(MessageBox.Cancel)
elif MessageBox.No & buttons:
box.setRejectButton(MessageBox.No)
return box.exec_()
def resizeEvent(self, event):
if not self.isMaximized():
self._current_size = event.size()
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def exec_(self):
if self.text in IGNORED_ERRORS:
self.accept()
return self.result()
return QDialog.exec_(self)
def accept(self):
if self.result() == 0:
if self._accept_button is not None:
self.setResult(self._accept_button)
else:
self.setResult(1)
self.accepted.emit()
if self.isModal():
self.hide()
def reject(self):
if self.result() == 0:
if self._reject_button is not None:
self.setResult(self._reject_button)
self.rejected.emit()
self.hide()
def hideEvent(self, event):
# event.ignore()
self.close()
def closeEvent(self, event):
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% create buttons %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def _create_buttons(self, buttons):
if MessageBox.Ok & buttons:
self._accept_button = MessageBox.Ok
bt = QPushButton(self.tr("&ok"))
bt.clicked.connect(self._on_ok_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Open & buttons:
self._accept_button = MessageBox.Open
bt = QPushButton(self.tr("&Open"))
bt.clicked.connect(self._on_open_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Save & buttons:
self._accept_button = MessageBox.Save
bt = QPushButton(self.tr("&Save"))
bt.clicked.connect(self._on_save_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Cancel & buttons:
self._reject_button = MessageBox.Cancel
bt = QPushButton(self.tr("&Cancel"))
bt.clicked.connect(self._on_cancel_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Close & buttons:
self._reject_button = MessageBox.Close
bt = QPushButton(self.tr("&Close"))
bt.clicked.connect(self._on_close_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Discard & buttons:
bt = QPushButton(self.tr("&Discard"))
bt.clicked.connect(self._on_discard_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.DestructiveRole)
if MessageBox.Apply & buttons:
bt = QPushButton(self.tr("&Apply"))
bt.clicked.connect(self._on_apply_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ApplyRole)
if MessageBox.Reset & buttons:
bt = QPushButton(self.tr("&Reset"))
bt.clicked.connect(self._on_reset_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ResetRole)
if MessageBox.RestoreDefaults & buttons:
bt = QPushButton(self.tr("&RestoreDefaults"))
bt.clicked.connect(self._on_restore_defaults_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ResetRole)
if MessageBox.Help & buttons:
bt = QPushButton(self.tr("&Help"))
bt.clicked.connect(self._on_help_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.HelpRole)
if MessageBox.SaveAll & buttons:
self._accept_button = MessageBox.SaveAll
bt = QPushButton(self.tr("&SaveAll"))
bt.clicked.connect(self._on_saveall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Yes & buttons:
bt = QPushButton(self.tr("&Yes"))
bt.clicked.connect(self._on_yes_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.YesRole)
if MessageBox.YesToAll & buttons:
bt = QPushButton(self.tr("&YesToAll"))
bt.clicked.connect(self._on_yestoall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.YesRole)
if MessageBox.No & buttons:
bt = QPushButton(self.tr("&No"))
bt.clicked.connect(self._on_no_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.NoRole)
if MessageBox.NoToAll & buttons:
bt = QPushButton(self.tr("&NoToAll"))
bt.clicked.connect(self._on_notoall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.NoRole)
if MessageBox.Abort & buttons:
self._reject_button = MessageBox.Abort
bt = QPushButton(self.tr("&Abort"))
bt.clicked.connect(self._on_abort_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Retry & buttons:
self._accept_button = MessageBox.Retry
bt = QPushButton(self.tr("&Retry"))
bt.clicked.connect(self._on_retry_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Ignore & buttons:
self._accept_button = MessageBox.Ignore
bt = QPushButton(self.tr("&Ignore"))
bt.clicked.connect(self._on_ignore_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Avoid & buttons:
if self._use_checkbox:
checkbox = QCheckBox("&Don't show again", self)
checkbox.stateChanged.connect(self._check_ignore)
self.buttonBox.addButton(checkbox, QDialogButtonBox.HelpRole)
else:
bt = QPushButton(self.tr("&Don't show again"))
bt.setMaximumHeight(24)
bt.clicked.connect(self._add_to_ignore)
self.buttonBox.addButton(bt, QDialogButtonBox.HelpRole)
def _on_ok_clicked(self):
self.done(MessageBox.Ok)
def _on_open_clicked(self):
self.done(MessageBox.Open)
def _on_save_clicked(self):
self.done(MessageBox.Save)
def _on_cancel_clicked(self):
self.done(MessageBox.Cancel)
def _on_close_clicked(self):
self.done(MessageBox.Close)
def _on_discard_clicked(self):
self.done(MessageBox.Discard)
def _on_apply_clicked(self):
self.done(MessageBox.Apply)
def _on_reset_clicked(self):
self.done(MessageBox.Reset)
def _on_restore_defaults_clicked(self):
self.done(MessageBox.RestoreDefaults)
def _on_help_clicked(self):
self.done(MessageBox.Help)
def _on_saveall_clicked(self):
self.done(MessageBox.SaveAll)
def _on_yes_clicked(self):
self.done(MessageBox.Yes)
def _on_yestoall_clicked(self):
self.done(MessageBox.YesToAll)
def _on_no_clicked(self):
self.done(MessageBox.No)
def _on_notoall_clicked(self):
self.done(MessageBox.NoToAll)
def _on_abort_clicked(self):
self.done(MessageBox.Abort)
def _on_retry_clicked(self):
self.done(MessageBox.Retry)
def _on_ignore_clicked(self):
self.done(MessageBox.Ignore)
def _add_to_ignore(self):
IGNORED_ERRORS.append(self.text)
self.accept()
def _check_ignore(self, state):
if state:
IGNORED_ERRORS.append(self.text)
else:
try:
IGNORED_ERRORS.remove(self.text)
except Exception:
pass
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher(
'pattern_generator/set_params',
PatternGeneratorParameters,
queue_size=1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.joystick_mode_checkbox)
# ignore invalid steps checkbox
self.ignore_invalid_steps_checkbox = QCheckBox()
self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
self.ignore_invalid_steps_checkbox.setChecked(False)
left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(
0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation,
"Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# signal connections
start_command.clicked.connect(self.start_command_callback)
stop_command.clicked.connect(self.stop_command_callback)
self.joystick_mode_checkbox.clicked.connect(
self.joystick_mode_check_callback)
self.ignore_invalid_steps_checkbox.clicked.connect(
self._publish_parameters)
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
move_group_name = rospy.get_param('~move_group', 'manipulator')
self.angle_delta = math.radians(
rospy.get_param('~rotation_delta_degrees', 25))
self.translation_delta = rospy.get_param('~translation_delta_meters',
0.1)
max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling',
0.5)
self.local_mover = CalibrationMovements(move_group_name,
max_velocity_scaling,
max_acceleration_scaling)
self.current_pose = -1
self.current_plan = None
self.initUI()
self.state = CalibrationMovementsGUI.NOT_INITED_YET
def initUI(self):
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/8')
self.bad_plan_lbl = QLabel('No plan yet')
self.guide_lbl = QLabel('Hello')
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def updateUI(self):
self.progress_bar.setMaximum(len(self.local_mover.poses))
self.progress_bar.setValue(self.current_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_pose + 1, len(self.local_mover.poses)))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
def handle_check_current_state(self):
self.local_mover.compute_poses_around_current_state(
self.angle_delta, self.translation_delta)
joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
math.radians(350)
] # TODO: make param
if self.local_mover.check_poses(joint_limits):
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_pose = -1
self.updateUI()
def handle_next_pose(self):
self.guide_lbl.setText('Going to center position')
if self.current_pose != -1:
plan = self.local_mover.plan_to_start_pose()
if plan is None:
self.guide_lbl.setText(
'Failed planning to center position: try again')
else:
self.local_mover.execute_plan(plan)
if self.current_pose < len(self.local_mover.poses) - 1:
self.current_pose += 1
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.updateUI()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
if self.current_pose >= 0:
self.current_plan = self.local_mover.plan_to_pose(
self.local_mover.poses[self.current_pose])
if CalibrationMovements.is_crazy_plan(
self.current_plan, self.local_mover.fallback_joint_limits
): #TODO: sort out this limits story
self.state = CalibrationMovementsGUI.BAD_PLAN
else:
self.state = CalibrationMovementsGUI.GOOD_PLAN
self.updateUI()
def handle_execute(self):
if self.current_plan is not None:
self.guide_lbl.setText('Going to the selected pose')
self.local_mover.execute_plan(self.current_plan)
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
self.updateUI()
class RoomDialogPlugin(Plugin):
def __init__(self, context):
super(RoomDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RoomDialogPlugin')
font_size = rospy.get_param("~font_size", 30)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QGridLayout()
self._layout.addLayout(self._button_layout)
# rospy.loginfo("Hello world")
# Add combobox
self._cb_layout = QHBoxLayout()
self._cb = QComboBox()
self._layout.addLayout(self._cb_layout)
#layout = QVBoxLayout(self._widget)
#layout.addWidget(self._button)
self._widget.setObjectName('RoomDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('room_dialog', RoomDialog,
self.service_callback)
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
# Add combo options
self.connect(self._widget, SIGNAL("update"), self.update)
self.connect(self._widget, SIGNAL("timeout"), self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self._widget.emit(SIGNAL("update"))
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return RoomDialogResponse(RoomDialogRequest.PREEMPTED, "")
if req.timeout != RoomDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self._widget.emit(SIGNAL("timeout"))
return RoomDialogResponse(RoomDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
req = self.request
self._text_browser.setText(req.message)
if req.type == RoomDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = RoomDialogResponse(RoomDialogRequest.NO_RESPONSE,
"")
self.response_ready = True
elif req.type == RoomDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
row = index / 3
col = index % 3
self._button_layout.addWidget(button, row, col)
self.buttons.append(button)
elif req.type == RoomDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label, 0, 0)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input, 0, 1)
# add handling of combobox
elif req.type == RoomDialogRequest.COMBOBOX_QUESTION:
# self.clean()
rospy.loginfo("Combobox selected")
#self._cb.duplicatesEnabled = False
if self._cb.count() == 0:
for index, options in enumerate(req.options):
self._cb.addItem(options)
rospy.loginfo(options)
#self.buttons.append(options)
# NOTE COULD INTRODUCE BUG
self._cb.currentIndexChanged.connect(self.handle_cb)
self._cb_layout.addWidget(self._cb)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
# while self._cb_layout.count():
# item = self._cb_layout.takeAt(0)
# item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = RoomDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = RoomDialogResponse(RoomDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def handle_cb(self, index):
# This will be the sign format seen around building ex: 3.404
rospy.loginfo("handling cb")
roomHuman = self._cb.currentText()
# modify string into robot format ex: d3_404
splitHuman = roomHuman.split('.', 1)
roomRobot = 'd' + splitHuman[0] + '_' + splitHuman[1]
roomRobot = str(roomRobot)
self.response = RoomDialogResponse(RoomDialogRequest.CB_RESPONSE,
roomRobot)
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
CHECKING_STARTING_POSITION = 6
MOVEMENT_FAILED = 7
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
self.handeye_client = HandeyeClient()
self.current_target_pose = -1 # -1 is home
self.target_poses = None
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.NOT_INITED_YET
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/0')
self.bad_plan_lbl = QLabel('No plan yet')
self.bad_plan_lbl.setAlignment(Qt.AlignCenter)
self.guide_lbl = QLabel('Hello')
self.guide_lbl.setWordWrap(True)
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def update_ui(self):
if self.target_poses:
count_target_poses = len(self.target_poses)
else:
count_target_poses = 1
self.progress_bar.setMaximum(count_target_poses)
self.progress_bar.setValue(self.current_target_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_target_pose + 1, count_target_poses))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.CHECKING_STARTING_POSITION:
self.guide_lbl.setText(
'Checking if the robot can translate and rotate in all directions from the current pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
QCoreApplication.processEvents()
def handle_check_current_state(self):
self.state = CalibrationMovementsGUI.CHECKING_STARTING_POSITION
self.update_ui()
res = self.handeye_client.check_starting_pose()
if res.can_calibrate:
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.update_ui()
def handle_next_pose(self):
res = self.handeye_client.select_target_pose(self.current_target_pose +
1)
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.update_ui()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
res = self.handeye_client.plan_to_selected_target_pose()
self.plan_was_successful = res.success
if self.plan_was_successful:
self.state = CalibrationMovementsGUI.GOOD_PLAN
else:
self.state = CalibrationMovementsGUI.BAD_PLAN
self.update_ui()
def handle_execute(self):
if self.plan_was_successful:
self.guide_lbl.setText('Going to the selected pose')
res = self.handeye_client.execute_plan()
if res.success:
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
else:
self.state = CalibrationMovementsGUI.MOVEMENT_FAILED
self.update_ui()
class NavViewWidget(QWidget):
def __init__(self, map_topic='/map',
paths=['/move_base/NavFn/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint']):
super(NavViewWidget, self).__init__()
self._layout = QVBoxLayout()
self._button_layout = QHBoxLayout()
self.setAcceptDrops(True)
self.setWindowTitle('Navigation Viewer')
self.paths = paths
self.polygons = polygons
self.map = map_topic
self._tf = tf.TransformListener()
self._nav_view = NavView(map_topic, paths, polygons, tf = self._tf, parent = self)
self._set_pose = QPushButton('Set Pose')
self._set_pose.clicked.connect(self._nav_view.pose_mode)
self._set_goal = QPushButton('Set Goal')
self._set_goal.clicked.connect(self._nav_view.goal_mode)
self._button_layout.addWidget(self._set_pose)
self._button_layout.addWidget(self._set_goal)
self._layout.addLayout(self._button_layout)
self._layout.addWidget(self._nav_view)
self.setLayout(self._layout)
def dragEnterEvent(self, e):
if not e.mimeData().hasText():
if not hasattr(e.source(), 'selectedItems') or len(e.source().selectedItems()) == 0:
qWarning('NavView.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = e.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning('NavView.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
else:
topic_name = str(e.mimeData().text())
if accepted_topic(topic_name):
e.accept()
e.acceptProposedAction()
def dropEvent(self, e):
if e.mimeData().hasText():
topic_name = str(e.mimeData().text())
else:
droped_item = e.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
topic_type, array = get_field_type(topic_name)
if not array:
if topic_type is OccupancyGrid:
self.map = topic_name
# Swap out the nav view for one with the new topics
self._nav_view.close()
self._nav_view = NavView(self.map, self.paths, self.polygons, self._tf, self)
self._layout.addWidget(self._nav_view)
elif topic_type is Path:
self.paths.append(topic_name)
self._nav_view.add_path(topic_name)
elif topic_type is PolygonStamped:
self.polygons.append(topic_name)
self._nav_view.add_polygon(topic_name)
def save_settings(self, plugin_settings, instance_settings):
self._nav_view.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._nav_view.restore_settings(plugin_settings, instance_settings)
