class TabGroup(GroupWidget):
def __init__(self, parent, updater, config, nodename):
super(TabGroup, self).__init__(updater, config, nodename)
self.parent = parent
if not self.parent.tab_bar:
self.parent.tab_bar = QTabWidget()
# Don't process wheel events when not focused
self.parent.tab_bar.tabBar().installEventFilter(self)
self.wid = QWidget()
self.wid.setLayout(self.grid)
parent.tab_bar.addTab(self.wid, self.param_name)
def eventFilter(self, obj, event):
if event.type() == QEvent.Wheel and not obj.hasFocus():
return True
return super(GroupWidget, self).eventFilter(obj, event)
def display(self, grid):
if not self.parent.tab_bar_shown:
grid.addRow(self.parent.tab_bar)
self.parent.tab_bar_shown = True
def close(self):
super(TabGroup, self).close()
self.parent.tab_bar = None
self.parent.tab_bar_shown = False
class PID_Tuner(Plugin):
'''
PID tunning widget for the velocity and steering PID controllers.
'''
def __init__(self, context):
'''
Initialie the PID tuner
Parameters:
N/A
Returns:
N/A
'''
#Initialize with qt_gui plugin for ros
super(PID_Tuner, self).__init__(context)
self.layout = QVBoxLayout()
self._widget = QWidget()
self._widget.setLayout(self.layout)
#Import a generic pid tunning widget for each pid controller
self.velocity_pid_tuner_widget = PID_Tuner_Generic_Widget()
self.steering_pid_tuner_widget = PID_Tuner_Generic_Widget()
self.pid_tuner_widgets = [
self.velocity_pid_tuner_widget, self.steering_pid_tuner_widget
]
#Add button for sending the gains
self.update_gains_btn = QPushButton("Update Gains")
self.update_gains_btn.clicked.connect(self._update_gains)
#Add the pid tuners to a layout
for pid_tuner_widget in self.pid_tuner_widgets:
self.layout.addWidget(pid_tuner_widget)
self.layout.addWidget(self.update_gains_btn)
context.add_widget(self._widget)
def _update_gains(self):
'''
Update the PID gains on the robot by sending them to the robot through
the service.
Parameters:
N/A
Returns:
N/A
'''
vel_k_p = self.velocity_pid_tuner_widget.k_p
vel_k_i = self.velocity_pid_tuner_widget.k_i
vel_k_d = self.velocity_pid_tuner_widget.k_d
steer_k_p = self.steering_pid_tuner_widget.k_p
steer_k_i = self.steering_pid_tuner_widget.k_i
steer_k_d = self.steering_pid_tuner_widget.k_d
update_pid_gains = rospy.ServiceProxy('smile/update_pid_gains',
pid_gains)
update_pid_gains(vel_k_p, vel_k_i, vel_k_d, steer_k_p, steer_k_i,
steer_k_d)
def __init__(self, check_state_changed_callback, topic_name, parent=None, is_topic = False):
super(TreeWidgetItem, self).__init__(parent)
self._check_state_changed_callback = check_state_changed_callback
self._topic_name = topic_name
self.setCheckState(CHECK_COLUMN, Qt.Unchecked)
self._is_topic = is_topic
self._slider = QSlider(Qt.Horizontal)
self._hbox = QHBoxLayout()
self._min_label = QLabel("min")
self._max_label = QLabel("max")
self._hbox.addWidget(self._min_label)
self._hbox.addWidget(self._slider)
self._hbox.addWidget(self._max_label)
tree = self.treeWidget()
widget = QWidget()
widget.setLayout(self._hbox)
# tree.setItemWidget(self, 3, widget)
self.setTextAlignment(self._column_names.index("min"), Qt.AlignRight | Qt.AlignVCenter)
self.setTextAlignment(self._column_names.index("max"), Qt.AlignLeft | Qt.AlignVCenter)
self.setTextAlignment(self._column_names.index("value"), Qt.AlignHCenter | Qt.AlignVCenter)
self.setTextAlignment(self._column_names.index("type"), Qt.AlignHCenter | Qt.AlignVCenter)
self.setFlags(Qt.ItemIsEditable | Qt.ItemIsEnabled | Qt.ItemIsSelectable | Qt.ItemIsUserCheckable )
self._slider.valueChanged.connect(self.sliderValueChanged)
class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
class NodeSelection(QWidget):
def __init__(self, parent):
super(NodeSelection, self).__init__()
self.parent_widget = parent
self.selected_nodes = []
self.setWindowTitle("Select the nodes you want to record")
self.resize(500, 700)
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Done", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.node_list = rosnode.get_node_names()
self.node_list.sort()
for node in self.node_list:
self.addCheckBox(node)
self.main_widget.setLayout(self.selection_vlayout)
self.show()
def addCheckBox(self, node):
item = QCheckBox(node, self)
item.stateChanged.connect(lambda x: self.updateNode(x, node))
self.selection_vlayout.addWidget(item)
def updateNode(self, state, node):
if state == Qt.Checked:
self.selected_nodes.append(node)
else:
self.selected_nodes.remove(node)
if len(self.selected_nodes) > 0:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
def onButtonClicked(self):
master = rosgraph.Master('rqt_bag_recorder')
state = master.getSystemState()
subs = [
t for t, l in state[1] if len([
node_name
for node_name in self.selected_nodes if node_name in l
]) > 0
]
# print subs
# print len(subs)
for topic in subs:
self.parent_widget.changeTopicCheckState(topic, Qt.Checked)
self.parent_widget.updateList(Qt.Checked, topic, topic)
self.close()
def add_item_to_conversation_view(self, msg, type=0):
label_msg = QLabel(msg)
label_msg.setWordWrap(True)
label_msg.setStyleSheet('font-size:10pt;')
inner_text_layout = QHBoxLayout()
horizonalSpacer1 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
inner_text_layout.addWidget(label_msg)
inner_text_layout.addItem(horizonalSpacer1)
elif type == 1:
inner_text_layout.addItem(horizonalSpacer1)
inner_text_layout.addWidget(label_msg)
inner_layout = QVBoxLayout()
time_msg = QLabel(str(time.asctime(time.localtime(time.time()))))
time_msg.setStyleSheet('font-size:8pt;')
inner_layout.addItem(inner_text_layout)
inner_layout.addWidget(time_msg)
inner_layout.setSizeConstraint(QLayout.SetFixedSize)
outer_layout = QHBoxLayout()
horizonalSpacer2 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
label_msg.setStyleSheet(
'background: #e5e5ea; padding: 6px; border-radius: 8px;')
time_msg.setAlignment(Qt.AlignLeft)
outer_layout.addItem(inner_layout)
outer_layout.addItem(horizonalSpacer2)
elif type == 1:
label_msg.setStyleSheet(
'background: #1d86f4; padding: 6px; border-radius: 8px; color:#fff;'
)
time_msg.setAlignment(Qt.AlignRight)
outer_layout.addItem(horizonalSpacer2)
outer_layout.addItem(inner_layout)
outer_layout.setSizeConstraint(QLayout.SetMinimumSize)
widget = QWidget()
widget.setLayout(outer_layout)
widget.resize(widget.sizeHint())
item = QListWidgetItem()
item.setSizeHint(widget.sizeHint())
self._widget.listWidget.addItem(item)
self._widget.listWidget.setItemWidget(item, widget)
self._widget.listWidget.scrollToBottom()
class NodeSelection(QWidget):
def __init__(self, parent):
super(NodeSelection, self).__init__()
self.parent_widget = parent
self.selected_nodes = []
self.setWindowTitle("Select the nodes you want to record")
self.resize(500, 700)
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Done", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.node_list = rosnode.get_node_names()
self.node_list.sort()
for node in self.node_list:
self.addCheckBox(node)
self.main_widget.setLayout(self.selection_vlayout)
self.show()
def addCheckBox(self, node):
item = QCheckBox(node, self)
item.stateChanged.connect(lambda x: self.updateNode(x, node))
self.selection_vlayout.addWidget(item)
def updateNode(self, state, node):
if state == Qt.Checked:
self.selected_nodes.append(node)
else:
self.selected_nodes.remove(node)
if len(self.selected_nodes) > 0:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
def onButtonClicked(self):
master = rosgraph.Master('rqt_bag_recorder')
state = master.getSystemState()
subs = [t for t, l in state[1]
if len([node_name for node_name in self.selected_nodes if node_name in l]) > 0]
for topic in subs:
self.parent_widget.changeTopicCheckState(topic, Qt.Checked)
self.parent_widget.updateList(Qt.Checked, topic)
self.close()
def process(self, action):
"""
:param action: action to execute, ''QAction''
:raises: when it doesn't recognice the action passed in, ''Exception''
"""
if action == self._reset_timeline:
self.timeline._timeline_frame.reset_timeline()
elif action == self._play_all:
self.timeline.toggle_play_all()
elif action == self._publish_all:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.start_publishing(topic):
break
elif action == self._publish_none:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.stop_publishing(topic):
break
elif action == self._thumbnail_show_action:
self.timeline._timeline_frame.set_renderers_active(True)
elif action == self._thumbnail_hide_action:
self.timeline._timeline_frame.set_renderers_active(False)
elif action in self._thumbnail_actions:
if self.timeline._timeline_frame.is_renderer_active(action.text()):
self.timeline._timeline_frame.set_renderer_active(
action.text(), False)
else:
self.timeline._timeline_frame.set_renderer_active(
action.text(), True)
elif action in self._topic_actions + self._type_actions:
popup_name = action.parentWidget().title() + '__' + action.text()
if popup_name not in self.timeline.popups:
frame = QWidget()
layout = QVBoxLayout()
frame.setLayout(layout)
frame.resize(640, 480)
viewer_type = action.data()
frame.setObjectName(popup_name)
view = viewer_type(self.timeline, frame)
self.timeline.popups.add(popup_name)
self.timeline.get_context().add_widget(frame)
self.timeline.add_view(action.parentWidget().title(), view,
frame)
frame.show()
elif action in self._publish_actions:
if self.timeline.is_publishing(action.text()):
self.timeline.stop_publishing(action.text())
else:
self.timeline.start_publishing(action.text())
else:
raise Exception('Unknown action in TimelinePopupMenu.process')
def __init__(self):
super(GroundStationWidget, self).__init__()
self._principle_layout = QBoxLayout(0) # main layout is horizontal (0)
self._principle_layout = QSplitter(Qt.Horizontal)
self._map_layout = QVBoxLayout()
map_widget = QWidget()
map_widget.setLayout(self._map_layout)
self._principle_layout.addWidget(map_widget)
self._control_layout = QVBoxLayout()
control_widget = QWidget()
control_widget.setLayout(self._control_layout)
self._principle_layout.addWidget(control_widget)
self.setAcceptDrops(False) # Dragging and Dropping not permitted
self.setWindowTitle('ROSplane Ground Station')
#=============================
self._mw = MapWindow()
self._map_layout.addWidget(self._mw)
self._tv = PlotWidget()
self._data_plot = DataPlot(self._tv)
self._data_plot.set_autoscale(x=False)
self._data_plot.set_autoscale(y=DataPlot.SCALE_EXTEND
| DataPlot.SCALE_VISIBLE)
self._data_plot.set_xlim([0, 10.0])
self._tv.switch_data_plot_widget(self._data_plot)
self._control_layout.addWidget(
self._tv,
1) # ratio of these numbers determines window proportions
self._ah = ArtificialHorizon()
self._control_layout.addWidget(self._ah, 1)
#=============================
print('fake init')
total_layout = QBoxLayout(QBoxLayout.TopToBottom)
self._principle_layout.setHandleWidth(8)
total_layout.addWidget(self._principle_layout)
self.setLayout(total_layout)
# Global timer for marble_map and artificial_horizon
self.interval = 100 # in milliseconds, period of regular update
self.timer = QTimer(self)
self.timer.setInterval(self.interval)
self.timer.timeout.connect(self._mw._marble_map.update)
self.timer.timeout.connect(self._ah.update)
self.timer.start()
def _create_cell_push_button(self, text, clicked_cb, icon=None):
widget = QWidget()
button = QPushButton()
button.setText(text)
if icon:
button.setIcon(icon)
button.clicked[bool].connect(clicked_cb)
hlayout = QHBoxLayout(widget)
hlayout.addWidget(button)
hlayout.setAlignment(Qt.AlignCenter)
hlayout.setContentsMargins(0, 0, 0, 0)
widget.setLayout(hlayout)
return widget
class TabGroup(GroupWidget):
def __init__(self, parent, updater, config, nodename):
super(TabGroup, self).__init__(updater, config, nodename)
self.parent = parent
if not self.parent.tab_bar:
self.parent.tab_bar = QTabWidget()
self.wid = QWidget()
self.wid.setLayout(self.grid)
parent.tab_bar.addTab(self.wid, self.param_name)
def display(self, grid):
if not self.parent.tab_bar_shown:
grid.addRow(self.parent.tab_bar)
self.parent.tab_bar_shown = True
def close(self):
super(TabGroup, self).close()
self.parent.tab_bar = None
self.parent.tab_bar_shown = False
class TabGroup(GroupWidget):
def __init__(self, parent, updater, config, nodename):
super(TabGroup, self).__init__(updater, config, nodename)
self.parent = parent
if not self.parent.tab_bar:
self.parent.tab_bar = QTabWidget()
self.wid = QWidget()
self.wid.setLayout(self.grid)
parent.tab_bar.addTab(self.wid, self.param_name)
def display(self, grid):
if not self.parent.tab_bar_shown:
grid.addRow(self.parent.tab_bar)
self.parent.tab_bar_shown = True
def close(self):
super(TabGroup, self).close()
self.parent.tab_bar = None
self.parent.tab_bar_shown = False
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.from_nodes_button = QPushButton("From Nodes", self)
self.from_nodes_button.clicked.connect(self.onFromNodesButtonClicked)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.main_vlayout.addWidget(self.from_nodes_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_monitor = QCheckBox("Monitor", self)
self.item_monAge = QCheckBox("Monitor + Agents", self)
self.item_all.stateChanged.connect(self.updateList)
self.item_monitor.stateChanged.connect(lambda x: self.updateList(x, "Monitor"))
self.item_monAge.stateChanged.connect(lambda x: self.updateList(x, "Monitor+"))
self.selection_vlayout.addWidget(self.item_all)
self.selection_vlayout.addWidget(self.item_monitor)
self.selection_vlayout.addWidget(self.item_monAge)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x, topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def changeTopicCheckState(self, topic, state):
for item in self.items_list:
if item.text() == topic:
item.setCheckState(state)
return
def updateList(self, state, topic=None, force_update_state=False):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
elif topic == "Monitor" or topic == "Monitor+":
if state == Qt.Checked:
self.item_monitor.setTristate(False)
self.selected_topics.append("/world_model")
self.selected_topics.append("/draws")
self.selected_topics.append("/debugs")
self.selected_topics.append("/referee")
for item in self.items_list:
if item.text() == "/world_model" \
or item.text() == "/debugs" \
or item.text() == "/draws" \
or item.text() =="/referee":
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
self.selected_topics.remove("/world_model")
self.selected_topics.remove("/draws")
self.selected_topics.remove("/debugs")
self.selected_topics.remove("/referee")
for item in self.items_list:
if item.text() == "/world_model" \
or item.text() =="/referee" \
or item.text() =="/debugs" \
or item.text()=="/draws":
item.setCheckState(Qt.Unchecked)
if topic == "Monitor+":
if state == Qt.Checked:
for item in self.items_list:
if item.text().find("agent") >-1:
self.selected_topics.append(item)
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
for item in self.items_list:
if item.text().find("agent") >-1:
self.selected_topics.remove(item)
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState() == Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit(
self.item_all.checkState() == Qt.Checked, self.selected_topics)
def onFromNodesButtonClicked(self):
self.node_selection = NodeSelection(self)
# combo_completer.setCompletionMode(QCompleter.InlineCompletion)
combo.lineEdit().setCompleter(combo_completer)
model_tree = QTreeView()
model_tree.setModel(combo_completer.model())
model_tree.expandAll()
for column in range(combo_completer.model().columnCount()):
model_tree.resizeColumnToContents(column)
completion_tree = QTreeView()
completion_tree.setModel(combo_completer.completionModel())
completion_tree.expandAll()
for column in range(combo_completer.completionModel().columnCount()):
completion_tree.resizeColumnToContents(column)
layout.addWidget(model_tree)
layout.addWidget(completion_tree)
layout.addWidget(edit)
layout.addWidget(combo)
layout.setStretchFactor(model_tree, 1)
widget.setLayout(layout)
mw.setCentralWidget(widget)
mw.move(300, 0)
mw.resize(800, 900)
mw.show()
app.exec_()
topic_completer_node.destroy_node()
rclpy.shutdown()
def __init__(self, context, node=None):
"""
This class is intended to be called by rqt plugin framework class.
Currently (12/12/2012) the whole widget is splitted into 2 panes:
one on left allows you to choose the node(s) you work on. Right side
pane lets you work with the parameters associated with the node(s) you
select on the left.
(12/27/2012) Despite the pkg name is changed to rqt_reconfigure to
reflect the available functionality, file & class names remain
'param', expecting all the parameters will become handle-able.
"""
super(ParamWidget, self).__init__()
self.setObjectName(self._TITLE_PLUGIN)
self.setWindowTitle(self._TITLE_PLUGIN)
rp = rospkg.RosPack()
#TODO: .ui file needs to replace the GUI components declaration
# below. For unknown reason, referring to another .ui files
# from a .ui that is used in this class failed. So for now,
# I decided not use .ui in this class.
# If someone can tackle this I'd appreciate.
_hlayout_top = QHBoxLayout(self)
_hlayout_top.setContentsMargins(QMargins(0, 0, 0, 0))
self._splitter = QSplitter(self)
_hlayout_top.addWidget(self._splitter)
_vlayout_nodesel_widget = QWidget()
_vlayout_nodesel_side = QVBoxLayout()
_hlayout_filter_widget = QWidget(self)
_hlayout_filter = QHBoxLayout()
self._text_filter = TextFilter()
self.filter_lineedit = TextFilterWidget(self._text_filter, rp)
self.filterkey_label = QLabel("&Filter key:")
self.filterkey_label.setBuddy(self.filter_lineedit)
_hlayout_filter.addWidget(self.filterkey_label)
_hlayout_filter.addWidget(self.filter_lineedit)
_hlayout_filter_widget.setLayout(_hlayout_filter)
self._nodesel_widget = NodeSelectorWidget(self, rp, self.sig_sysmsg)
_vlayout_nodesel_side.addWidget(_hlayout_filter_widget)
_vlayout_nodesel_side.addWidget(self._nodesel_widget)
_vlayout_nodesel_side.setSpacing(1)
_vlayout_nodesel_widget.setLayout(_vlayout_nodesel_side)
reconf_widget = ParameditWidget(rp)
self._splitter.insertWidget(0, _vlayout_nodesel_widget)
self._splitter.insertWidget(1, reconf_widget)
# 1st column, _vlayout_nodesel_widget, to minimize width.
# 2nd col to keep the possible max width.
self._splitter.setStretchFactor(0, 0)
self._splitter.setStretchFactor(1, 1)
# Signal from paramedit widget to node selector widget.
reconf_widget.sig_node_disabled_selected.connect(
self._nodesel_widget.node_deselected)
# Pass name of node to editor widget
self._nodesel_widget.sig_node_selected.connect(
reconf_widget.show_reconf)
if not node:
title = self._TITLE_PLUGIN
else:
title = self._TITLE_PLUGIN + ' %s' % node
self.setObjectName(title)
#Connect filter signal-slots.
self._text_filter.filter_changed_signal.connect(
self._filter_key_changed)
# Open any clients indicated from command line
self.sig_selected.connect(self._nodesel_widget.node_selected)
for rn in [rospy.resolve_name(c) for c in context.argv()]:
if rn in self._nodesel_widget.get_paramitems():
self.sig_selected.emit(rn)
else:
rospy.logwarn('Could not find a dynamic reconfigure client named \'%s\'', str(rn))
class Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.items():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
def __init__(self, context, node=None):
"""
This class is intended to be called by rqt plugin framework class.
Currently (12/12/2012) the whole widget is splitted into 2 panes:
one on left allows you to choose the node(s) you work on. Right side
pane lets you work with the parameters associated with the node(s) you
select on the left.
(12/27/2012) Despite the pkg name is changed to rqt_reconfigure to
reflect the available functionality, file & class names remain
'param', expecting all the parameters will become handle-able.
"""
super(ParamWidget, self).__init__()
self.setObjectName(self._TITLE_PLUGIN)
self.setWindowTitle(self._TITLE_PLUGIN)
rp = rospkg.RosPack()
#TODO: .ui file needs to replace the GUI components declaration
# below. For unknown reason, referring to another .ui files
# from a .ui that is used in this class failed. So for now,
# I decided not use .ui in this class.
# If someone can tackle this I'd appreciate.
_hlayout_top = QHBoxLayout(self)
_hlayout_top.setContentsMargins(QMargins(0, 0, 0, 0))
self._splitter = QSplitter(self)
_hlayout_top.addWidget(self._splitter)
_vlayout_nodesel_widget = QWidget()
_vlayout_nodesel_side = QVBoxLayout()
_hlayout_filter_widget = QWidget(self)
_hlayout_filter = QHBoxLayout()
self._text_filter = TextFilter()
self.filter_lineedit = TextFilterWidget(self._text_filter, rp)
self.filterkey_label = QLabel("&Filter key:")
self.filterkey_label.setBuddy(self.filter_lineedit)
_hlayout_filter.addWidget(self.filterkey_label)
_hlayout_filter.addWidget(self.filter_lineedit)
_hlayout_filter_widget.setLayout(_hlayout_filter)
self._nodesel_widget = NodeSelectorWidget(self, rp, self.sig_sysmsg)
_vlayout_nodesel_side.addWidget(_hlayout_filter_widget)
_vlayout_nodesel_side.addWidget(self._nodesel_widget)
_vlayout_nodesel_side.setSpacing(1)
_vlayout_nodesel_widget.setLayout(_vlayout_nodesel_side)
reconf_widget = ParameditWidget(rp)
self._splitter.insertWidget(0, _vlayout_nodesel_widget)
self._splitter.insertWidget(1, reconf_widget)
# 1st column, _vlayout_nodesel_widget, to minimize width.
# 2nd col to keep the possible max width.
self._splitter.setStretchFactor(0, 0)
self._splitter.setStretchFactor(1, 1)
# Signal from paramedit widget to node selector widget.
reconf_widget.sig_node_disabled_selected.connect(
self._nodesel_widget.node_deselected)
# Pass name of node to editor widget
self._nodesel_widget.sig_node_selected.connect(
reconf_widget.show_reconf)
if not node:
title = self._TITLE_PLUGIN
else:
title = self._TITLE_PLUGIN + ' %s' % node
self.setObjectName(title)
#Connect filter signal-slots.
self._text_filter.filter_changed_signal.connect(
self._filter_key_changed)
# Open any clients indicated from command line
self.sig_selected.connect(self._nodesel_widget.node_selected)
for rn in [rospy.resolve_name(c) for c in context.argv()]:
if rn in self._nodesel_widget.get_paramitems():
self.sig_selected.emit(rn)
else:
rospy.logwarn(
'Could not find a dynamic reconfigure client named \'%s\'',
str(rn))
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.items():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class KillCounterPlugin(Plugin):
def __init__(self, context):
super(KillCounterPlugin, self).__init__(context)
#return
# Give QObjects reasonable names
self.setObjectName('KillCounterPlugin')
rp = rospkg.RosPack()
# Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-q", "--quiet", action="store_true",
# dest="quiet",
# help="Put plugin in silent mode")
#args, unknowns = parser.parse_known_args(context.argv())
#if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._num_killed = 0
self._kill_label = QLabel('Martians Killed: 0')
#Killing Martians is Bad Tho, We are a dying species and killing us is mean and will destroy an awesome society with culture and life and happiness. I'm so disappointed in you Kimberly
self._layout.addWidget(self._kill_label)
MIN = 540
MAX = 660
self._chance = randint(MIN, MAX)
context.add_widget(self._container)
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelection.ui')
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelectionSimple.ui')
# Extend the widget with all attributes and children from UI file
#loadUi(ui_file, self._widget, {})
# Give QObjects reasonable names
self._widget.setObjectName('Kill Counter')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
#if context.serial_number() > 1:
#self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self.running = True
rate = rospy.Rate(1)
def run():
while self.running:
num = randint(0, self._chance)
if num == self._chance:
self._num_killed += 1
self._kill_label.setText('Martians Killed: ' +
str(self._num_killed))
rate.sleep()
self.run_thread = threading.Thread(target=run)
self.run_thread.start()
def shutdown_plugin(self):
# TODO unregister all publishers here`
self.running = False
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_all.stateChanged.connect(self.updateList)
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x,topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def updateList(self, state, topic = None):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState()==Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit((self.item_all.checkState() == Qt.Checked), self.selected_topics)
class SystemWidget(QWidget):
"""
main class inherits from the ui window class.
You can specify the topics that the topic pane.
SystemWidget.start must be called in order to update topic pane.
"""
_column_names = ['topic', 'type', 'bandwidth', 'rate', 'value']
def __init__(self, plugin=None):
"""
"""
super(SystemWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_agent'), 'resource',
'SystemWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self._subsystems = {}
self.all_subsystems = {}
self._widgets = {}
self.prev_subsystems = []
self.levels_layouts = []
self.structure_root = None
self.structure_graph = None
self.structure_changed = False
self.scrollArea = QScrollArea()
self.scrollArea.setWidgetResizable(True)
self.horizontalLayout.addWidget(self.scrollArea)
self.mainWidget = QWidget()
self.scrollArea.setWidget(self.mainWidget)
self.verticalLayout = QVBoxLayout()
self.mainWidget.setLayout(self.verticalLayout)
# self._tree_items = {}
# self._column_index = {}
# for column_name in self._column_names:
# self._column_index[column_name] = len(self._column_index)
# self.refresh_topics()
# init and start update timer
self._timer_refresh_topics = QTimer(self)
self._timer_refresh_topics.timeout.connect(self.refresh_topics)
def checkStructureChange(self):
result = False
for subsystem_name in self.prev_subsystems:
if not subsystem_name in self._widgets:
result = True
break
if result == False:
for subsystem_name in self._widgets:
if not subsystem_name in self.prev_subsystems:
result = True
break
self.prev_subsystems = []
for subsystem_name in self._widgets:
self.prev_subsystems.append(subsystem_name)
return result
def start(self):
"""
This method needs to be called to start updating topic pane.
"""
self._timer_refresh_topics.start(100)
def generateStructure(self):
names = []
for w1_name in self._widgets:
names.append(w1_name)
self._widgets[w1_name].resetBuffersLayout()
for w1_name in self._widgets:
w1 = self._widgets[w1_name]
if not w1.isInitialized():
continue
for w2_name in self._widgets:
w2 = self._widgets[w2_name]
if not w2.isInitialized():
continue
common_buffers = w1.getCommonBuffers(w2)
if common_buffers != None:
w1.groupBuffers(common_buffers, w2.subsystem_name)
w2.groupBuffers(common_buffers, w1.subsystem_name)
parents_dict = {}
for w1_name in self._widgets:
w1 = self._widgets[w1_name]
if not w1.isInitialized():
continue
for w2_name in self._widgets:
w2 = self._widgets[w2_name]
if not w2.isInitialized():
continue
rel_pose = w1.getLowerSubsystemPosition(w2.subsystem_name)
if rel_pose != -1:
parents_dict[w2_name] = w1_name
# print parents_dict
# get top-most subsystem (root)
root = None
if parents_dict:
root = list(parents_dict.keys())[0]
while root in parents_dict:
root = parents_dict[root]
levels = []
if root == None:
# there are no levels
one_level = []
for w_name in self._widgets:
one_level.append(w_name)
if len(one_level) > 0:
levels.append(one_level)
else:
levels.append([root])
while True:
# expand all subsystems in the lowest level
current_lowest = levels[-1]
next_lower_level = []
for s in current_lowest:
lower_list = self._widgets[s].getLowerSubsystems()
next_lower_level = next_lower_level + lower_list
if len(next_lower_level) == 0:
break
else:
levels.append(next_lower_level)
# TODO: manage disjoint trees
added_subsystems = []
for l in levels:
for s in l:
added_subsystems.append(s)
for w_name in self._widgets:
if not w_name in added_subsystems:
print "WARNING: subsystem %s is not in the main tree. This is not implemented." % (
w_name)
print "levels:", levels
return levels
def layout_widgets(self, layout):
return (layout.itemAt(i) for i in range(layout.count()))
@Slot()
def refresh_topics(self):
"""
refresh tree view items
"""
#
# update the list of subsystems
#
topic_list = rospy.get_published_topics()
if topic_list is None:
rospy.logerr(
'Not even a single published topic found. Check network configuration'
)
return
# start new topic dict
new_subsystems = {}
for topic_name, topic_type in topic_list:
name_split = topic_name.split('/')
# print name_split
if (len(name_split) == 3) and (name_split[0] == '') and (
name_split[2]
== 'diag') and (topic_type
== "diagnostic_msgs/DiagnosticArray"):
subsystem_name = name_split[1]
# if topic is new
if subsystem_name not in self._subsystems:
# create new TopicInfo
topic_info = TopicInfo(topic_name, topic_type)
new_subsystems[subsystem_name] = topic_info
topic_info.start_monitoring()
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_subsystems[subsystem_name] = self._subsystems[
subsystem_name]
del self._subsystems[subsystem_name]
# remove unused subsystems
while True:
repeat = False
for s in self._subsystems:
if not s in new_subsystems:
del self._subsystems[s]
repeat = True
break
if not repeat:
break
# switch to new topic dict
self._subsystems = new_subsystems
# print self._subsystems.keys()
#
# update each subsystem
#
new_widgets = {}
for subsystem_name in self._subsystems:
msg = self._subsystems[subsystem_name].last_message
if (msg != None) and (len(msg.status) == 2) and \
msg.status[0].name == 'components' and msg.status[1].name == 'diagnostics':
name_split = subsystem_name.split('/')
if not subsystem_name in self.all_subsystems:
self.all_subsystems[subsystem_name] = SubsystemWidget(
self._plugin, subsystem_name)
if not subsystem_name in self._widgets:
new_widgets[subsystem_name] = self.all_subsystems[
subsystem_name]
# self.verticalLayout.addWidget(new_widgets[subsystem_name])
else:
new_widgets[subsystem_name] = self._widgets[subsystem_name]
# del self._widgets[subsystem_name]
# for value in msg.status[1].values:
# if value.key == 'master_component':
# new_widgets[subsystem_name].setStateName(value.value, '')
# break
# remove unused subsystems
# while True:
# repeat = False
# for s in self._widgets:
# if not s in new_widgets:
# del self._widgets[s]
# repeat = True
# break
# if not repeat:
# break
self._widgets = new_widgets
# print self._widgets.keys()
structure_changed = self.checkStructureChange()
if structure_changed:
self.structure_changed = True
if self.structure_changed:
allInitialized = True
for subsystem_name in self._widgets:
if not self._widgets[subsystem_name].isInitialized():
allInitialized = False
break
if allInitialized:
# remove all widgets from layouts
# and remove all layouts
for i in reversed(range(len(self.levels_layouts))):
layout = self.levels_layouts[i]
for i in reversed(range(layout.count())):
# The new widget is deleted when its parent is deleted.
layout.itemAt(i).widget().setParent(None)
self.verticalLayout.removeItem(layout)
del layout
self.levels_layouts = []
levels = self.generateStructure()
for l in levels:
hbox = QHBoxLayout()
for w in l:
hbox.addWidget(self._widgets[w])
self._widgets[w].show()
self.levels_layouts.append(hbox)
self.verticalLayout.addLayout(hbox)
# for
# TODO
self.structure_changed = False
while True:
repeat = False
for s in self.all_subsystems:
if not s in self._widgets:
del self.all_subsystems[s]
repeat = True
break
if not repeat:
break
for subsystem_name in self._widgets:
self._widgets[subsystem_name].update_subsystem(
self._subsystems[subsystem_name].last_message)
def shutdown_plugin(self):
for topic in self._topics.values():
topic['info'].stop_monitoring()
self._timer_refresh_topics.stop()
def set_selected_topics(self, selected_topics):
"""
@param selected_topics: list of tuple. [(topic_name, topic_type)]
@type selected_topics: []
"""
rospy.logdebug('set_selected_topics topics={}'.format(
len(selected_topics)))
self._selected_topics = selected_topics
# TODO(Enhancement) Save/Restore tree expansion state
def save_settings(self, plugin_settings, instance_settings):
header_state = self.topics_tree_widget.header().saveState()
instance_settings.set_value('tree_widget_header_state', header_state)
def restore_settings(self, pluggin_settings, instance_settings):
if instance_settings.contains('tree_widget_header_state'):
header_state = instance_settings.value('tree_widget_header_state')
if not self.topics_tree_widget.header().restoreState(header_state):
rospy.logwarn("rqt_topic: Failed to restore header state.")
class EventTransmissionGUI(Plugin):
"""
GUI to send events from User to logic state machine
"""
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
def createSlider(self, minimum, maximum, default_value, tick_interval):
"""
Create a QtSlider with specified properties
Parameters:
@param minimum Minimum value for slider
@param maximum Maximum value for slider
@param default_value Initial value the slider is set to
@param tick_inverval Integer value specifying difference between
successive ticks
@return Slider value
"""
slider = QSlider(Qt.Horizontal)
slider.setMinimum(minimum)
slider.setMaximum(maximum)
slider.setValue(default_value)
slider.setTickPosition(QSlider.TicksBelow)
slider.setTickInterval(tick_interval)
return slider
def _parse_args(self, argv):
"""
Parse extra arguments when plugin is deployed in standalone mode
"""
parser = argparse.ArgumentParser(prog='aerial_autonomy',
add_help=False)
EventTransmissionGUI.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
"""
Notify rqt_gui that this plugin can parse these extra arguments
"""
group = parser.add_argument_group(
'Options for aerial autonomy gui plugin')
group.add_argument("-e",
"--event_file",
type=str,
default='',
help="Event file")
group.add_argument("-c",
"--grid_cols",
type=int,
default=3,
help="Number of columns in grid")
def get_row_col(self, button_index, ncols):
"""
Automatically find the row and col to add the button
to in a grid based on index of the button
"""
col_index = button_index % ncols
row_index = int((button_index - col_index) / ncols)
return (row_index, col_index)
def poseCommandCallback(self, pose):
"""
Saves pose command and updates command display
"""
self.pose_command = pose
self.pose_x.setText("x: {0:.2f}".format(
self.pose_command.pose.position.x))
self.pose_y.setText("y: {0:.2f}".format(
self.pose_command.pose.position.y))
def poseCommandButtonCallback(self):
"""
Publishes stored pose command after setting height from slider
"""
if self.pose_command:
self.pose_command.pose.position.z = self.height_slider.value()
self.event_trigger.triggerPoseCommand(self.pose_command)
# Reset pose command to avoid accidental triggering
self.pose_command = None
self.pose_x.setText('x: -')
self.pose_y.setText('y: -')
else:
print "No pose command to trigger"
def velocityCommandButtonCallback(self):
"""
Publishes stored velocity command
"""
velocity_command = VelocityYaw()
velocity_command.vx = self.velocity_sliders[0].value() / 10.0
velocity_command.vy = self.velocity_sliders[1].value() / 10.0
velocity_command.vz = self.velocity_sliders[2].value() / 10.0
velocity_command.yaw = self.velocity_sliders[3].value() * np.pi / 180.0
self.event_trigger.triggerVelocityCommand(velocity_command)
def updateLabel(self, header, label, slider, scale=1):
"""
Updates height label based on slider value
"""
label.setText(header + ": {0:.2f}".format(slider.value() / scale))
def updateStatus(self, status, text_box):
"""
Generic placeholder function to update text box
"""
if not sip.isdeleted(text_box):
text_box.setHtml(status)
doc_size = text_box.document().size()
text_box.setFixedHeight(doc_size.height() + 10)
class BatteryMonitoringPlugin(Plugin):
def voltage_callback(self, data):
self._voltage_label.setText('Voltage: ' + str(data.data) + ' V')
def current_callback(self, data):
self._current_label.setText('Current: ' + str(data.data) + ' A')
def current_drawn_callback(self, data):
self._current_drawn_label.setText('Current Drawn: ' + str(data.data) +
' Ah')
def __init__(self, context):
super(BatteryMonitoringPlugin, self).__init__(context)
#return
# Give QObjects reasonable names
self.setObjectName('BatteryMonitoringPlugin')
rp = rospkg.RosPack()
# Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-q", "--quiet", action="store_true",
# dest="quiet",
# help="Put plugin in silent mode")
#args, unknowns = parser.parse_known_args(context.argv())
#if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._voltage_label = QLabel('Voltage: ')
self._current_label = QLabel('Current: ')
self._current_drawn_label = QLabel('Current Drawn: ')
self._layout.addWidget(self._voltage_label)
self._layout.addWidget(self._current_label)
self._layout.addWidget(self._current_drawn_label)
context.add_widget(self._container)
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelection.ui')
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelectionSimple.ui')
# Extend the widget with all attributes and children from UI file
#loadUi(ui_file, self._widget, {})
# Give QObjects reasonable names
self._widget.setObjectName('Battery Monitoring')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
#if context.serial_number() > 1:
#self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self.running = True
rate = rospy.Rate(1)
def run():
rospy.Subscriber("bms/voltage", Float64, self.voltage_callback)
rospy.Subscriber("bms/current", Float64, self.current_callback)
rospy.Subscriber("bms/current_drawn", Float64,
self.current_drawn_callback)
while not self.running:
rate.sleep()
self.run_thread = threading.Thread(target=run)
self.run_thread.start()
def shutdown_plugin(self):
# TODO unregister all publishers here
self.running = False
self.found_service = True
class NodeSelection(QWidget):
def __init__(self, parent):
super(NodeSelection, self).__init__()
self.parent_widget = parent
self.selected_nodes = []
self.setWindowTitle("Select the nodes you want to record")
self.resize(300, 300)
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Done", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.node_list = rosnode.get_node_names()
self.node_list.sort()
for node in self.node_list:
# add planner node by choosing agent number
if "planner" not in node:
self.addCheckBox(node)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, node):
item = QCheckBox(node, self)
item.stateChanged.connect(lambda x: self.updateNode(x, node))
self.selection_vlayout.addWidget(item)
def updateNode(self, state, node):
if state == Qt.Checked:
self.selected_nodes.append(node)
if "planner" + "_" + node.split("_")[1] in self.node_list:
self.selected_nodes.append("planner" + "_" +
node.split("_")[1])
else:
self.selected_nodes.remove(node)
if "planner" + "_" + node.split("_")[1] in self.node_list:
self.selected_nodes.remove("planner" + "_" +
node.split("_")[1])
if len(self.selected_nodes) > 0:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
def onButtonClicked(self):
master = rosgraph.Master('rqt_bag_recorder')
state = master.getSystemState()
print state[1]
subs = [
t for t, l in state[1] if len([
node_name
for node_name in self.selected_nodes if node_name in l
]) > 0
]
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
if ("/" + topic.split("/")[1] + "/task" in subs):
subs.append(topic)
for topic in subs:
self.parent_widget.changeTopicCheckState(topic, Qt.Checked)
self.parent_widget.updateList(Qt.Checked, topic)
self.close()
class HandEyeCalibration(Plugin):
PLUGIN_TITLE = ' Intel OTC Robotics: Hand-Eye Calibration'
def __init__(self, context):
super(HandEyeCalibration, self).__init__(context)
self.context = context
self.node = context.node
self.widget = QWidget()
self.widget.setObjectName(self.PLUGIN_TITLE)
self.widget.setWindowTitle(self.PLUGIN_TITLE)
# Data
self.Tsamples = []
# Toolbar
_, path_pkg = get_resource('packages', 'handeye_dashboard')
print("{}".format(path_pkg))
self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
'Take a snapshot', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/capture.png'
self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'Get the camera/robot transform',
self.widget)
self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
'Clear the record data.', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'EStart the publishing the TF.',
self.widget)
self.toolbar = QToolBar()
self.toolbar.addAction(self.snapshot_action)
self.toolbar.addAction(self.calibrate_action)
self.toolbar.addAction(self.clear_action)
self.toolbar.addAction(self.execut_action)
# Toolbar0
self.l0 = QLabel(self.widget)
self.l0.setText("Camera-Mount-Type: ")
self.l0.setFixedWidth(150)
self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.combobox = QComboBox(self.widget)
self.combobox.addItem('attached on robot')
self.combobox.addItem('fixed beside robot')
self.toolbar0 = QToolBar()
self.toolbar0.addWidget(self.l0)
self.toolbar0.addWidget(self.combobox)
# Toolbar1
self.l1 = QLabel(self.widget)
self.l1.setText("Camera-Frame: ")
self.l1.setFixedWidth(150)
self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.camera_frame = QLineEdit(self.widget)
self.camera_frame.setText("camera_link")
self.toolbar1 = QToolBar()
self.toolbar1.addWidget(self.l1)
self.toolbar1.addWidget(self.camera_frame)
# Toolbar2
self.l2 = QLabel(self.widget)
self.l2.setText("Object-Frame: ")
self.l2.setFixedWidth(150)
self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.object_frame = QLineEdit(self.widget)
self.object_frame.setText("calib_board")
self.toolbar2 = QToolBar()
self.toolbar2.addWidget(self.l2)
self.toolbar2.addWidget(self.object_frame)
# Toolbar3
self.l3 = QLabel(self.widget)
self.l3.setText("Robot-Base-Frame: ")
self.l3.setFixedWidth(150)
self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.base_frame = QLineEdit(self.widget)
self.base_frame.setText("base")
self.toolbar3 = QToolBar()
self.toolbar3.addWidget(self.l3)
self.toolbar3.addWidget(self.base_frame)
# Toolbar4
self.l4 = QLabel(self.widget)
self.l4.setText("End-Effector-Frame: ")
self.l4.setFixedWidth(150)
self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.endeffector_frame = QLineEdit(self.widget)
self.endeffector_frame.setText("tool0")
self.toolbar4 = QToolBar()
self.toolbar4.addWidget(self.l4)
self.toolbar4.addWidget(self.endeffector_frame)
# Toolbar5
self.l5 = QLabel(self.widget)
self.l5.setText("Sample-Number: ")
self.l5.setFixedWidth(150)
self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.le5 = QLineEdit(self.widget)
self.le5.setValidator(QIntValidator())
self.le5.setText('10')
self.le5.setReadOnly(True)
self.toolbar5 = QToolBar()
self.toolbar5.addWidget(self.l5)
self.toolbar5.addWidget(self.le5)
# TreeView
self.treeview = QTreeView()
self.treeview.setAlternatingRowColors(True)
self.model = QStandardItemModel(self.treeview)
self.treeview.setModel(self.model)
self.treeview.setHeaderHidden(True)
# TextEdit
self.textedit = QTextEdit(self.widget)
self.textedit.setReadOnly(True)
# Layout
self.layout = QVBoxLayout()
self.layout.addWidget(self.toolbar0)
self.layout.addWidget(self.toolbar1)
self.layout.addWidget(self.toolbar2)
self.layout.addWidget(self.toolbar3)
self.layout.addWidget(self.toolbar4)
self.layout.addWidget(self.toolbar5)
self.layout.addWidget(self.toolbar)
self.layoutH = QHBoxLayout()
self.layoutH.addWidget(self.treeview)
self.layoutH.addWidget(self.textedit)
self.layout.addLayout(self.layoutH)
self.widget.setLayout(self.layout)
# Add the widget to the user interface
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.widget)
# Make the connections
self.snapshot_action.triggered.connect(self.take_snapshot)
self.calibrate_action.triggered.connect(self.calibration)
self.clear_action.triggered.connect(self.clear)
self.execut_action.triggered.connect(self.execution)
# Package path
self.path_pkg = path_pkg
# Set up TF
self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
self.req = HandeyeTF.Request()
def clear(self):
# >>> Clear the recorded samples
self.textedit.append('Clearing the recorded data ...')
self.textedit.clear()
self.Tsamples = []
self.model.clear()
def get_tf_transform(self, frame_id, child_frame_id):
self.req.transform.header.frame_id = frame_id
self.req.transform.child_frame_id = child_frame_id
self.req.publish.data = False
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
transform = TransformStamped()
try:
result = future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
transform = result.tf_lookup_result
return transform
def publish_tf_transform(self, transform_to_publish):
self.req.publish.data = True
self.req.transform = transform_to_publish
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
try:
future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
self.node.get_logger().info(
'Send the camera-robot transform :\n\tfrom `{}` to `{}`.'.
format(self.req.transform.header.frame_id,
self.req.transform.child_frame_id))
def take_snapshot(self):
# >>> Take the snapshot
self.textedit.append('Taking snapshot ...')
# Get the transform from `tool0` to `base_link`
T = self.get_tf_transform(self.base_frame.text(),
self.endeffector_frame.text())
bTe = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
bTe = br.quaternion.to_transform(q)
bTe[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.base_frame.text(), self.endeffector_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'bTe:' + bcolors.ENDC +
'\n{}'.format(bTe))
# Get the transform from `calib_board` to `camera_link`
T = self.get_tf_transform(self.camera_frame.text(),
self.object_frame.text())
cTo = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
cTo = br.quaternion.to_transform(q)
cTo[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.camera_frame.text(), self.object_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'cTo:' + bcolors.ENDC +
'\n{}'.format(cTo))
parent = QStandardItem('Snapshot {}'.format(len(self.Tsamples)))
child_1 = QStandardItem('bTe:\n{}\n{}\n{}\n{}'.format(
bTe[0, :], bTe[1, :], bTe[2, :], bTe[3, :]))
child_2 = QStandardItem('cTo:\n{}\n{}\n{}\n{}'.format(
cTo[0, :], cTo[1, :], cTo[2, :], cTo[3, :]))
parent.appendRow(child_1)
parent.appendRow(child_2)
self.model.appendRow(parent)
self.Tsamples.append((bTe, cTo))
self.le5.setText(str(len(self.Tsamples)))
def calibration(self):
# >>> Compute the calibration
self.textedit.append('Making the calibration ...')
if len(self.Tsamples) == 0:
self.textedit.append(
'No transform recorded, please take snapshots.')
return
# save samples to `dataset.json` file
save_samples_to_file(self.Tsamples)
import handeye
if self.combobox.currentIndex() == 0:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Moving')
if self.combobox.currentIndex() == 1:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Fixed')
for sample in self.Tsamples:
solver_cri.add_sample(sample[0], sample[1])
try:
bTc = solver_cri.solve(method=handeye.solver.Daniilidis1999)
# save the calibration result to 'camera-robot.json' file
file_output = '/tmp/' + 'camera-robot.json'
with open(file_output, 'w') as f:
json.dump(bTc.tolist(), f)
except Exception:
self.textedit.append("Failed to solve the hand-eye calibration.")
def execution(self):
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def shutdown_plugin(self):
"""
Unregister subscribers when the plugin shutdown
"""
pass
def save_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
def restore_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
class SpectrogramPlugin(Plugin):
update_signal = QtCore.pyqtSignal()
subscriber_signal = QtCore.pyqtSignal(str)
module_num_signal = QtCore.pyqtSignal()
value_signal = QtCore.pyqtSignal(int)
vib_freq = 48000
overlap = 50
stft_freq = 200
frame_time = 1
frame_length = frame_time * stft_freq
label_flag = 0
lowcut_freq = 0
highcut_freq = vib_freq / 2
v_max = 0
v_min = 0
colorbarflag = 0
cnt_fig = 0
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
sns.set(style="whitegrid", palette="bright", color_codes=True)
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.lbl_topic = QLabel('Topic:')
layout_.addWidget(self.lbl_topic)
self.le_topic = QLineEdit()
layout_.addWidget(self.le_topic)
self.apply_topic = QPushButton("Apply")
self.apply_topic.clicked.connect(self.apply_clicked_topic)
layout_.addWidget(self.apply_topic)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_lcf = QLabel('Low-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_lcf)
self.spb_lcf = QSpinBox()
self.spb_lcf.setRange(0, 50)
self.spb_lcf.setValue(0)
layout_.addWidget(self.spb_lcf)
self.apply_lcf = QPushButton("Apply")
self.apply_lcf.clicked.connect(self.apply_clicked_lcf)
layout_.addWidget(self.apply_lcf)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_hcf = QLabel('High-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_hcf)
self.spb_hcf = QSpinBox()
self.spb_hcf.setRange(50, self.vib_freq / 2)
self.spb_hcf.setValue(self.vib_freq / 2)
layout_.addWidget(self.spb_hcf)
self.apply_hcf = QPushButton("Apply")
self.apply_hcf.clicked.connect(self.apply_clicked_hcf)
layout_.addWidget(self.apply_hcf)
layout.addLayout(layout_)
#self.fig, self.axes = plt.subplots(2, 1, sharex=True)
self.fig = plt.figure()
self.ax = self.fig.add_subplot(1, 1, 1)
self.canvas = FigureCanvas(self.fig)
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('spectrum')
def changed_spinbox_value(self, n):
self.valueChanged.emit(n)
def spectrum_callback(self, data):
nch = data.nch
len = data.nfreq
if self.spectrogram is None:
self.spectrogram = zeros([len, self.frame_length, nch])
#self.spectrogram = ones([len, self.frame_length, 4*nch])*log(1e-8)
self.spectrogram = roll(self.spectrogram, -1, 1)
for i in range(nch):
s = array(data.data).reshape([nch, len, 2])[i]
s = linalg.norm(s, axis=1)
s += 1e-8
log(s, s)
self.spectrogram[:, -1, i] = s
#if data.header.seq % 2 == 0:
if self.v_max < self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max():
self.v_max = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max()
if self.v_min > self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min():
self.v_min = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min()
self.update_signal.emit()
def apply_clicked_topic(self):
self.update_subscriber(self.le_topic.displayText())
def apply_clicked_lcf(self):
self.lowcut_freq = self.spb_lcf.value()
def apply_clicked_hcf(self):
self.highcut_freq = self.spb_hcf.value()
def update_spectrogram(self):
if self.spectrogram is not None:
yticks = [
0, self.highcut_freq / 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / 2 * 3 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq * 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq
]
yticks_label = [
self.lowcut_freq, self.highcut_freq / 4, self.highcut_freq / 2,
self.highcut_freq / 4 * 3, self.highcut_freq
]
xticks = [
0, self.frame_length / 4 - 1, self.frame_length / 2 - 1,
self.frame_length * 3 / 4 - 1, self.frame_length - 1
]
xticks_label = [
-self.frame_time * 2, -self.frame_time / 2 * 3,
-self.frame_time, -self.frame_time / 2, 0
]
font_size = 13
if self.cnt_fig % 40 == 0:
self.ax.clear()
#self.im = self.ax.imshow(self.spectrogram[int(self.lowcut_freq*100/self.overlap/self.stft_freq):int(self.highcut_freq*100/self.overlap/self.stft_freq)+1,:,0], aspect="auto", origin="lower", cmap="winter", interpolation='none', vmin=self.v_min, vmax=self.v_max)
self.im = self.ax.imshow(self.spectrogram[
int(self.lowcut_freq * 100 / self.overlap /
self.stft_freq):int(self.highcut_freq * 100 /
self.overlap / self.stft_freq) +
1, :, 0],
aspect="auto",
origin="lower",
cmap="jet",
interpolation='none',
vmin=12,
vmax=16)
self.ax.grid(None)
self.ax.set_ylabel("Freq. [Hz]",
fontsize=font_size,
fontname='serif')
self.ax.set_yticks(yticks)
self.ax.set_yticklabels(["$%.1f$" % y for y in yticks_label],
fontsize=font_size)
self.ax.set_xticks(xticks)
self.ax.set_xticklabels(["$%.1f$" % x for x in xticks_label],
fontsize=font_size)
self.ax.set_xlabel("Time [s]",
fontsize=font_size,
fontname='serif')
if self.colorbarflag == 0:
self.colorbarflag = 1
self.cb = self.fig.colorbar(self.im)
elif self.colorbarflag == 1:
self.cb.update_bruteforce(self.im)
self.canvas.draw()
self.cnt_fig += 1
QApplication.processEvents()
def update_subscriber(self, topic_name):
self.topic_name = topic_name
self.le_topic.setText(self.topic_name)
if hasattr(self, 'sub'):
self.sub.unregister()
self.spectrogram = None
#self.topic_name = 'spectrum'
self.sub = rospy.Subscriber(topic_name,
Spectrum,
self.spectrum_callback,
queue_size=5)
def shutdown_plugin(self):
pass
def createEditor(self, parent, option, index):
print option
print index.column()
indices = [2, 3, 4]
if index.column() not in indices:
return None
print "create_edit"
if index.column() == 3:
widget = QWidget(parent)
slider = QSlider(Qt.Horizontal, widget)
text_edit = QLineEdit(widget)
text_edit.setFocus()
text_edit.selectAll()
text_edit.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
hbox = QVBoxLayout()
# slider.setMinimumWidth(400)
# hbox.setMargin(0);
hbox.setContentsMargins(2,1,2,1);
# min_label = QLabel("min")
# max_label = QLabel("max")
# vbox.addWidget(min_label)
hbox.addWidget(slider)
hbox.addWidget(text_edit)
# vbox.addWidget(max_label)
widget.setLayout(hbox)
# widget.setMinimumWidth(400)
slider.setMinimumHeight(32)
text_edit.setMinimumHeight(36)
widget.setMinimumHeight(80)
widget.setAutoFillBackground(True)
widget.setStyleSheet(".QWidget { background:rgb(200, 200, 200); margin:0px; border:1px solid rgb(170, 170, 170); }")
# parent.setMinimumWidth(300)
row = index.row()
imin = index.sibling(row, self._column_names.index("min"))
imax = index.sibling(row, self._column_names.index("max"))
smin = index.model().data(imin, Qt.EditRole)
smax = index.model().data(imax, Qt.EditRole)
fmin = 0
fmax = 100
def isFloat(s):
try:
float(s)
return True
except:
return False
if isFloat(smin):
fmin = float(smin)
if isFloat(smax):
fmax = float(smax)
print fmin, fmax
slider.setMinimum(int(fmin * 100))
slider.setMaximum(int(fmax * 100))
slider.setMouseTracking(True)
@Slot(int)
def sliderValueChanged(value):
text_edit.setText(str(float(value)/100.))
print "slider value changed to %d" % value
text_edit.setFocus()
text_edit.selectAll()
pass
@Slot()
def sliderDown():
print "slider pressed"
@Slot()
def sliderUp():
print "slider released"
@Slot()
def editingFinished():
text = text_edit.text()
value = float(text)
nvalue = value
nvalue = min(nvalue, fmax)
nvalue = max(nvalue, fmin)
# print "nvalue is", nvalue
if value != nvalue:
text_edit.setText(str(nvalue))
svalue = int(nvalue*100)
print "text changed to %s" % text
print "nvalue is", nvalue
print "svalue is %d" % svalue
slider.blockSignals(True)
slider.setSliderPosition(svalue)
slider.blockSignals(False)
pass
@Slot(str)
def lineEditTextChanged(text):
# text_edit.setText(str(value))
# slider.setSliderDown(True)
# slider.sliderPressed.emit()
print "setSliderDown"
if not isFloat(text):
return
value = float(text)
nvalue = value
# nvalue = min(nvalue, fmax)
# nvalue = max(nvalue, fmin)
# print "nvalue is", nvalue
if value != nvalue:
text_edit.setText(str(nvalue))
svalue = int(nvalue*100)
print "text changed to %s" % text
print "nvalue is", nvalue
print "svalue is %d" % svalue
slider.blockSignals(True)
slider.setSliderPosition(svalue)
slider.blockSignals(False)
pass
slider.valueChanged.connect(sliderValueChanged)
slider.sliderPressed.connect(sliderDown)
slider.sliderReleased.connect(sliderUp)
text_edit.textChanged.connect(lineEditTextChanged)
text_edit.editingFinished.connect(editingFinished)
text_edit.selectAll()
text_edit.setFocus()
rx = QRegExp("^-?([1-9]\d*|0)(\.\d*)?$")
v = QRegExpValidator(rx)
text_edit.setValidator(v)
widget.setFocusProxy(text_edit)
edt = widget
else:
edt = QLineEdit(parent)
if index.column() == 2:
edt.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
elif index.column() == 4:
edt.setAlignment(Qt.AlignLeft | Qt.AlignVCenter)
rx = QRegExp("^-?([1-9]\d*|0)(\.\d*)?$")
v = QRegExpValidator(rx);
edt.setValidator(v)
# edt.setInputMask("#00000000")
return edt
class ExperimentGUI(Plugin):
def __init__(self, context):
super(ExperimentGUI, self).__init__(context)
# Give QObjects reasonable names
self.plans = [
'Starting Position', 'Above Panel', 'Panel Grabbed',
'Above Placement Nest', 'Panel Placed'
]
#state_dict ties each state to planlistindex values
#self.state_dict={'reset_position':0,'pickup_prepare':1,'pickup_lower':2,'pickup_grab_first_step':2,'pickup_grab_second_step':2,'pickup_raise':2,'transport_panel':3,'place_lower':4,'place_set_first_step':4,'place_set_second_step':4,'place_raise':4}
self.gui_execute_states = [
"reset", "panel_pickup", "pickup_grab", "transport_panel",
"place_panel"
]
self.execute_states = [
['plan_to_reset_position', 'move_to_reset_position'],
['plan_pickup_prepare', 'move_pickup_prepare'],
[
'plan_pickup_lower', 'move_pickup_lower',
'plan_pickup_grab_first_step', 'move_pickup_grab_first_step',
'plan_pickup_grab_second_step', 'move_pickup_grab_second_step',
'plan_pickup_raise', 'move_pickup_raise'
], ['plan_transport_payload', 'move_transport_payload'],
['plan_place_set_second_step']
]
self.teleop_modes = [
'Error', 'Off', 'Joint', 'Cartesian', 'Cylindrical', 'Spherical'
]
self.current_teleop_mode = 1
self.teleop_button_string = "Tele-op\nMode:\n"
self.setObjectName('MyPlugin')
self._lock = threading.Lock()
#self._send_event=threading.Event()
self.controller_commander = controller_commander_pkg.ControllerCommander(
)
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self.in_process = None
self.recover_from_pause = False
#rospy.get_param("rosbag_name")
#<param name="start_time" command="date +'%d-%m-%Y_%Ih%Mm%S'"/>
#rosbag record args="record -O arg('start_time')
self._mainwidget = QWidget()
self.layout = QGridLayout()
self._mainwidget.setLayout(self.layout)
self.disconnectreturnoption = False
self.stackedWidget = QStackedWidget() #self._mainwidget)
self.layout.addWidget(self.stackedWidget, 0, 0)
self._welcomescreen = QWidget()
self._runscreen = QWidget()
self._errordiagnosticscreen = QWidget()
self.stackedWidget.addWidget(self._welcomescreen)
self.stackedWidget.addWidget(self._runscreen)
self.stackedWidget.addWidget(self._errordiagnosticscreen)
#self._data_array=collections.deque(maxlen=500)
self._proxy_model = message_proxy_model.MessageProxyModel()
self._rospack = rospkg.RosPack()
#self.console=console_widget.ConsoleWidget(self._proxy_model,self._rospack)
self.robotconnectionled = LEDIndicator()
self.robotconnectionled.setDisabled(True) # Make the led non clickable
self.forcetorqueled = LEDIndicator()
self.forcetorqueled.setDisabled(True) # Make the led non clickable
self.overheadcameraled = LEDIndicator()
self.overheadcameraled.setDisabled(True) # Make the led non clickable
self.grippercameraled = LEDIndicator()
self.grippercameraled.setDisabled(True) # Make the led non clickable
self.runscreenstatusled = LEDIndicator()
self.runscreenstatusled.setDisabled(True)
self.step_executor = GUI_Step_Executor()
self.step_executor.error_signal.connect(self._feedback_receive)
#self.step_executor.error_function=self._feedback_receive
#Need this to pause motions
self.process_client = actionlib.ActionClient('process_step',
ProcessStepAction)
self.process_client.wait_for_server()
self.placement_targets = {
'leeward_mid_panel': 'panel_nest_leeward_mid_panel_target',
'leeward_tip_panel': 'panel_nest_leeward_tip_panel_target'
}
self.placement_target = 'panel_nest_leeward_mid_panel_target'
self.panel_type = 'leeward_mid_panel'
self.client_handle = None
service_list = rosservice.get_service_list()
if ('/overhead_camera/trigger' in service_list):
self.led_change(self.overheadcameraled, True)
else:
self.led_change(self.overheadcameraled, False)
if ('/gripper_camera_2/trigger' in service_list):
self.led_change(self.grippercameraled, True)
else:
self.led_change(self.grippercameraled, False)
self.mode = 0
#self.rewound=False
self.count = 0
self.data_count = 0
self.force_torque_data = np.zeros((6, 1))
self.joint_angle_data = np.zeros((6, 1))
# Get path to UI file which should be in the "resource" folder of this package
self.welcomescreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'welcomeconnectionscreen.ui')
self.runscreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'Runscreen.ui')
self.errorscreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'errordiagnosticscreen.ui')
self.rewound = False
# Extend the widget with all attributes and children from UI file
loadUi(self.welcomescreenui, self._welcomescreen)
loadUi(self.runscreenui, self._runscreen)
loadUi(self.errorscreenui, self._errordiagnosticscreen)
# Give QObjects reasonable names
self._mainwidget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._mainwidget.setWindowTitle(self._mainwidget.windowTitle() +
(' (%d)' %
context.serial_number()))
context.add_widget(self._mainwidget)
self.context = context
self.plugin_settings = None
self.instance_settings = None
#self._errordiagnosticscreen.consoleWidget=console_widget.ConsoleWidget(self._proxy_model,self._rospack)
#####consoleiagnosticscreen.backToRun.pressed.connect(self._to_run_screen)
#self._runscThread=ConsoleThread(self._widget.State_info)
# self._welcomescreen.statusFormLayout.takeAt(0)
self._welcomescreen.statusFormLayout.addWidget(self.robotconnectionled,
0, 0)
self._welcomescreen.statusFormLayout.addWidget(self.forcetorqueled, 2,
0)
self._welcomescreen.statusFormLayout.addWidget(self.overheadcameraled,
4, 0)
self._welcomescreen.statusFormLayout.addWidget(self.grippercameraled,
6, 0)
self._runscreen.connectionLayout.addWidget(self.runscreenstatusled, 0,
1)
#self._welcomescreen.robotConnectionWidget.addWidget(self.led)
#consoleThread.finished.connect(app.exit)
#####consoleThread.start()
self.rviz_starter = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'src', 'rpi_arm_composites_manufacturing_gui', 'rviz_starter.py')
# Add widget to the user interface
#context.add_widget(console)==QDialog.Accepted
#context.add_widget(rqt_console)
for entry in self.plans:
listentry = QListWidgetItem(entry)
listentry.setFlags(Qt.ItemIsSelectable)
self._runscreen.planList.addItem(listentry)
self._runscreen.planList.item(0).setSelected(True)
self.planListIndex = 0
self._runscreen.panelType.setText(self.panel_type)
self._runscreen.placementNestTarget.setText("Leeward Mid Panel Nest")
self._runscreen.panelType.setReadOnly(True)
self._runscreen.placementNestTarget.setReadOnly(True)
self.commands_sent = False
rospy.Subscriber("controller_state", controllerstate, self.callback)
self._set_controller_mode = rospy.ServiceProxy("set_controller_mode",
SetControllerMode)
rospy.Subscriber("GUI_state", ProcessState, self.process_state_set)
#rospy.Subscriber('gui_error', String, self._feedback_receive())
self.force_torque_plot_widget = QWidget()
self.joint_angle_plot_widget = QWidget()
self._welcomescreen.openConfig.clicked.connect(
self._open_config_options)
#self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_login_prompt)
self._welcomescreen.toRunScreen.pressed.connect(self._to_run_screen)
self._runscreen.backToWelcome.pressed.connect(self._to_welcome_screen)
#self._runscreen.toErrorScreen.pressed.connect(self._to_error_screen)
self._runscreen.nextPlan.pressed.connect(self._next_plan)
self._runscreen.previousPlan.pressed.connect(self._previousPlan)
self._runscreen.resetToHome.pressed.connect(self._reset_position)
self._runscreen.stopPlan.pressed.connect(self._stopPlan)
self._runscreen.accessTeleop.pressed.connect(self.change_teleop_modes)
#self._errordiagnosticscreen.openOverheadCameraView.pressed.connect(self._open_overhead_camera_view)
#self._errordiagnosticscreen.openGripperCameraViews.pressed.connect(self._open_gripper_camera_views)
self._errordiagnosticscreen.openForceTorqueDataPlot.pressed.connect(
self._open_force_torque_data_plot)
self._errordiagnosticscreen.openJointAngleDataPlot.pressed.connect(
self._open_joint_angle_data_plot)
self._errordiagnosticscreen.backToRun.pressed.connect(
self._to_run_screen)
#self._runscreen.widget.frame=rviz.VisualizationFrame()
#self._runscreen.widget.frame.setSplashPath( "" )
## VisualizationFrame.initialize() must be called before
## VisualizationFrame.load(). In fact it must be called
## before most interactions with RViz classes because it
## instantiates and initializes the VisualizationManager,
## which is the central class of RViz.
#self._runscreen.widget.frame.initialize()
#self.manager = self._runscreen.widget.frame.getManager()
# self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_advanced_options)
def led_change(self, led, state):
led.setChecked(state)
def _to_welcome_screen(self):
self.stackedWidget.setCurrentIndex(0)
def _to_run_screen(self):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
if (self.stackedWidget.currentIndex() == 0):
self.messagewindow = PanelSelectorWindow()
self.messagewindow.show()
self.messagewindow.setFixedSize(self.messagewindow.size())
if self.messagewindow.exec_():
next_selected_panel = self.messagewindow.get_panel_selected()
if (next_selected_panel != None):
self.panel_type = next_selected_panel
self.placement_target = self.placement_targets[
self.panel_type]
self.stackedWidget.setCurrentIndex(1)
self._runscreen.panelType.setText(self.panel_type)
if (self.panel_type == 'leeward_mid_panel'):
self._runscreen.placementNestTarget.setText(
"Leeward Mid Panel Nest")
elif (self.panel_type == 'leeward_tip_panel'):
self._runscreen.placementNestTarget.setText(
"Leeward Tip Panel Nest")
else:
raise Exception('Unknown panel type selected')
def _to_error_screen(self):
self.stackedWidget.setCurrentIndex(2)
def _login_prompt(self):
self.loginprompt = UserAuthenticationWindow()
if self.loginprompt.exec_():
#self.loginprompt.show()
#while(not self.loginprompt.returned):
#pass
return True
else:
return False
def _open_config_options(self):
if (self._login_prompt()):
self.led_change(self.robotconnectionled, True)
#def _open_overhead_camera_view(self):
#def _open_gripper_camera_views(self):
def _open_force_torque_data_plot(self):
self.plot_container = []
self.x_data = np.arange(1)
self.force_torque_app = QApplication([])
self.force_torque_plot_widget = pg.plot()
self.force_torque_plot_widget.addLegend()
#self.layout.addWidget(self.force_torque_plot_widget,0,1)
self.force_torque_plot_widget.showGrid(x=True, y=True)
self.force_torque_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m')
self.force_torque_plot_widget.setLabel('bottom', 'Sample Number', 'n')
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 0, 0),
name="Torque X"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 255, 0),
name="Torque Y"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 0, 255),
name="Torque Z"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 255, 0),
name="Force X"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 255, 255),
name="Force Y"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 0, 255),
name="Force Z"))
#self.force_torque_plotter=PlotManager(title='Force Torque Data',nline=3,widget=self.force_torque_plot_widget)
#self.force_torque_plot_widget.show()
#self.force_torque_plotter.add("Hello", np.arange(10))
#self.force_torque_plotter.update()
#self.rosGraph.show()
#self.rosGraph.exec_()
def _open_joint_angle_data_plot(self):
self.plot_container = []
self.x_data = np.arange(1)
self.joint_angle_app = QApplication([])
self.joint_angle_plot_widget = pg.plot()
self.joint_angle_plot_widget.addLegend()
#self.layout.addWidget(self.joint_angle_plot_widget,0,1)
self.joint_angle_plot_widget.showGrid(x=True, y=True)
self.joint_angle_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m')
self.joint_angle_plot_widget.setLabel('bottom', 'Sample Number', 'n')
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 0, 0), name="Joint 1"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 255, 0), name="Joint 2"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 0, 255), name="Joint 3"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 255, 0),
name="Joint 4"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 255, 255),
name="Joint 5"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 0, 255),
name="Joint 6"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 255, 255),
name="Joint 7"))
def _open_rviz_prompt(self):
subprocess.Popen(['python', self.rviz_starter])
# def _open_advanced_options(self):
# main = Main()
# sys.exit(main.main(sys.argv, standalone='rqt_rviz/RViz', plugin_argument_provider=add_arguments))
def _raise_rviz_window(self):
subprocess.call(["xdotool", "search", "--name", "rviz", "windowraise"])
def _next_plan(self):
#TODO: Disable previous plan if planListIndex==2 or 4
self._runscreen.nextPlan.setDisabled(True)
self._runscreen.previousPlan.setDisabled(True)
self._runscreen.resetToHome.setDisabled(True)
self.reset_teleop_button()
#TODO Make it change color when in motion
if (self.planListIndex + 1 == self._runscreen.planList.count()):
self.planListIndex = 0
elif (self.recover_from_pause):
self.recover_from_pause = False
else:
self.planListIndex += 1
#g=GUIStepGoal(self.gui_execute_states[self.planListIndex], self.panel_type)
#self.client_handle=self.client.send_goal(g,done_cb=self._process_done,feedback_cb=self._feedback_receive)
self.step_executor._nextPlan(self.panel_type, self.planListIndex,
self.placement_target)
self._runscreen.planList.item(self.planListIndex).setSelected(True)
if (self.rewound):
self.rewound = False
self._runscreen.previousPlan.setDisabled(False)
"""
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Not Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("Biased to 0")
self._runscreen.pressureSensor.setText("[0,0,0]")
"""
'''
elif(self.planListIndex==1):
self.send_thread=threading.Thread(target=self._execute_steps,args=(1,self.last_step, self.panel_type,0))
rospy.loginfo("thread_started")
self.send_thread.setDaemon(True)
self.send_thread.start()
self._send_event.set()
#self._execute_step('plan_pickup_prepare',self.panel_type)
#self._execute_step('move_pickup_prepare')''' """
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("ON")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[0,0,0]")
""" """
elif(self.planListIndex==2):
self.send_thread=threading.Thread(target=self._execute_steps,args=(2,self.last_step))
self.send_thread.setDaemon(True)
self.send_thread.start()
self._send_event.set()""" """
self._execute_step('plan_pickup_lower')
self._execute_step('move_pickup_lower')
self._execute_step('plan_pickup_grab_first_step')
self._execute_step('move_pickup_grab_first_step')
self._execute_step('plan_pickup_grab_second_step')
self._execute_step('move_pickup_grab_second_step')
self._execute_step('plan_pickup_raise')
self._execute_step('move_pickup_raise')
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Localized")self.controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander()
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[0,0,0]")
""" """
elif(self.planListIndex==3):
if(self.panel_type=="leeward_mid_panel"):
subprocess.Popen(['python', self.YC_transport_code, 'leeward_mid_panel'])
elif(self.panel_type=="leeward_tip_panel"):
subprocess.Popen(['python', self.YC_transport_code, 'leeward_tip_panel'])
self.commands_sent=True
"""
#self.send_thread=threading.Thread(target=self._execute_steps,args=(3,self.last_step,self.placement_target,0))
#self.send_thread.setDaemon(True)
#self.send_thread.start()
#self._send_event.set()"""
"""
self._execute_step('plan_transport_payload',self.placement_target)
self._execute_step('move_transport_payload')
self._runscreen.vacuum.setText("ON")
self._runscreen.panel.setText("Attached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[1,1,1]")
""" """
elif(self.planListIndex==4):
if(self.panel_type=="leeward_mid_panel"):
subprocess.Popen(['python', self.YC_place_code])
elif(self.panel_type=="leeward_tip_panel"):
subprocess.Popen(['python', self.YC_place_code2])
self.commands_sent=True
""" """
self._runscreen.vacuum.setText("ON")
self._runscreen.panel.setText("Attached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("OFF")
self._runscreen.pressureSensor.setText("[1,1,1]")
"""
def _stopPlan(self):
#self.client.cancel_all_goals()
#self.process_client.cancel_all_goals()
#g=GUIStepGoal("stop_plan", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive)
self.step_executor._stopPlan()
self.recover_from_pause = True
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
self.reset_teleop_button()
def _previousPlan(self):
if (self.planListIndex == 0):
self.planListIndex = self._runscreen.planList.count() - 1
else:
self.planListIndex -= 1
self.reset_teleop_button()
self._runscreen.planList.item(self.planListIndex).setSelected(True)
# self.rewound=True
#self._runscreen.previousPlan.setDisabled(True)
#g=GUIStepGoal("previous_plan", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive,done_cb=self._process_done)
self.step_executor._previousPlan()
@pyqtSlot()
def _feedback_receive(self):
with self._lock:
messagewindow = VacuumConfirm()
error_msg = self.step_executor.error
confirm = QMessageBox.warning(
messagewindow, 'Error',
'Operation failed with error:\n' + error_msg, QMessageBox.Ok,
QMessageBox.Ok)
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
def process_state_set(self, data):
#if(data.state!="moving"):
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
def _reset_position(self):
messagewindow = VacuumConfirm()
reply = QMessageBox.question(messagewindow, 'Path Verification',
'Proceed to Reset Position',
QMessageBox.Yes | QMessageBox.No,
QMessageBox.No)
if reply == QMessageBox.Yes:
self.planListIndex = 0
#g=GUIStepGoal("reset", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive)
self.reset_teleop_button()
self.step_executor._nextPlan(None, self.planListIndex)
self._runscreen.planList.item(self.planListIndex).setSelected(True)
#subprocess.Popen(['python', self.reset_code])
else:
rospy.loginfo("Reset Rejected")
def start_shared_control(self):
if (self.planListIndex + 1 == self._runscreen.planList.count()):
self.planListIndex = 0
elif self.recover_from_pause:
self.recover_from_pause = False
else:
self.planListIndex += 1
if (self.planListIndex == 1):
self._execute_step('plan_pickup_prepare', self.panel_type)
elif (self.planListIndex == 2):
self._execute_step('plan_pickup_lower')
elif (self.planListIndex == 3):
self._execute_step('plan_pickup_grab_first_step')
#TODO: How to handle what happens after threshold exceeded to generate next plan step
elif (self.planListIndex == 4):
self._execute_step('plan_pickup_raise')
elif (self.planListIndex == 5):
self._execute_step('plan_transport_payload', self.placement_target)
#self.set_controller_mode(self.controller_commander.MODE_SHARED_TRAJECTORY, 1, [],[])
def change_teleop_modes(self):
with self._lock:
self.current_teleop_mode += 1
try:
if (self.current_teleop_mode == len(self.teleop_modes)):
self.current_teleop_mode = 1
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
elif (self.current_teleop_mode == 1):
self.reset_teleop_button()
elif (self.current_teleop_mode == 2):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_JOINT_TELEOP, 1, [], [])
elif (self.current_teleop_mode == 3):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_CARTESIAN_TELEOP, 1, [],
[])
elif (self.current_teleop_mode == 4):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_CYLINDRICAL_TELEOP, 1,
[], [])
elif (self.current_teleop_mode == 5):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_SPHERICAL_TELEOP, 1, [],
[])
rospy.loginfo("Entering teleop mode:" +
self.teleop_modes[self.current_teleop_mode])
button_string = self.teleop_button_string + self.teleop_modes[
self.current_teleop_mode]
self._runscreen.accessTeleop.setText(button_string)
except Exception as err:
rospy.loginfo(str(err))
self.step_executor.error = "Controller failed to set teleop mode"
self.step_executor.error_signal.emit()
def set_controller_mode(self,
mode,
speed_scalar=1.0,
ft_bias=[],
ft_threshold=[]):
req = SetControllerModeRequest()
req.mode.mode = mode
req.speed_scalar = speed_scalar
req.ft_bias = ft_bias
req.ft_stop_threshold = ft_threshold
res = self._set_controller_mode(req)
if (res.error_code.mode != ControllerMode.MODE_SUCCESS):
raise Exception("Could not set controller mode")
def error_recovery_button(self):
self.current_teleop_mode = 0
self._runscreen.accessTeleop.setText("Recover from Error")
def reset_teleop_button(self):
self.current_teleop_mode = 1
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
button_string = self.teleop_button_string + self.teleop_modes[
self.current_teleop_mode]
self._runscreen.accessTeleop.setText(button_string)
def callback(self, data):
#self._widget.State_info.append(data.mode)
with self._lock:
if (self.stackedWidget.currentIndex() == 2):
if (self.count > 10):
#stringdata=str(data.mode)
#freeBytes.acquire()
#####consoleData.append(str(data.mode))
self._errordiagnosticscreen.consoleWidget_2.addItem(
str(data.joint_position))
self.count = 0
#print data.joint_position
self.count += 1
#self._widget.State_info.scrollToBottom()
#usedBytes.release()
#self._data_array.append(stringdata)
#print self._widget.State_info.count()
if (self._errordiagnosticscreen.consoleWidget_2.count() > 200):
item = self._errordiagnosticscreen.consoleWidget_2.takeItem(
0)
#print "Hello Im maxed out"
del item
'''
if self.in_process:
if self.client.get_state() == actionlib.GoalStatus.PENDING or self.client.get_state() == actionlib.GoalStatus.ACTIVE:
self._runscreen.nextPlan.setDisabled(True)
self._runscreen.previousPlan.setDisabled(True)
self._runscreen.resetToHome.setDisabled(True)
rospy.loginfo("Pending")
elif self.client.get_state() == actionlib.GoalStatus.SUCCEEDED:
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
self.in_process=False
rospy.loginfo("Succeeded")
elif self.client.get_state() == actionlib.GoalStatus.ABORTED:
self.in_process=False
if(not self.recover_from_pause):
raise Exception("Process step failed and aborted")
elif self.client.get_state() == actionlib.GoalStatus.REJECTED:
self.in_process=False
raise Exception("Process step failed and Rejected")
elif self.client.get_state() == actionlib.GoalStatus.LOST:
self.in_process=False
raise Exception("Process step failed and lost")
'''
if (self.count > 10):
self.count = 0
if (data.mode.mode < 0):
'''
#self.stackedWidget.setCurrentIndex(2)
if(data.mode.mode==-5 or data.mode.mode==-6):
error_msg="Error mode %d : Controller is not synched or is in Invalid State" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-3 or data.mode.mode==-2):
error_msg="Error mode %d : Controller operation or argument is invalid" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-13 or data.mode.mode==-14):
error_msg="Error mode %d : Sensor fault or communication Error" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-1):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -1: Internal system error detected")
if(data.mode.mode==-4):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -4: Robot Fault detected")
if(data.mode.mode==-7):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -7: Robot singularity detected, controller cannot perform movement")
if(data.mode.mode==-8):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -8: Robot Setpoint could not be tracked, robot location uncertain")
if(data.mode.mode==-9):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -9: Commanded Trajectory is invalid and cannot be executed. Please replan")
if(data.mode.mode==-10):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -10: Trajectory Tracking Error detected, robot position uncertain, consider lowering speed of operation to improve tracking")
if(data.mode.mode==-11):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -11: Robot trajectory aborted.")
if(data.mode.mode==-12):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -12: Robot Collision Imminent, operation stopped to prevent damage")
if(data.mode.mode==-15):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -15: Sensor state is invalid")
if(data.mode.mode==-16):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -16: Force Torque Threshold Violation detected, stopping motion to prevent potential collisions/damage")
if(data.mode.mode==-17):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -17: Invalid External Setpoint given")
'''
#messagewindow=VacuumConfirm()
#reply = QMessageBox.question(messagewindow, 'Connection Lost',
# 'Robot Connection Lost, Return to Welcome Screen?', QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
#if reply==QMessageBox.Yes:
#
# self.disconnectreturnoption=False
#else:
# self.disconnectreturnoption=True
self.led_change(self.robotconnectionled, False)
self.led_change(self.runscreenstatusled, False)
self.error_recovery_button()
else:
self.led_change(self.robotconnectionled, True)
self.led_change(self.runscreenstatusled, True)
if (data.ft_wrench_valid == "False"):
self.stackedWidget.setCurrentIndex(0)
self.led_change(self.forcetorqueled, False)
else:
self.led_change(self.forcetorqueled, True)
#self.service_list=rosservice.get_service_list()
#if(self.disconnectreturnoption and data.error_msg==""):
# self.disconnectreturnoption=False
self.count += 1
if (not self.force_torque_plot_widget.isHidden()):
self.x_data = np.concatenate((self.x_data, [data.header.seq]))
incoming = np.array([
data.ft_wrench.torque.x, data.ft_wrench.torque.y,
data.ft_wrench.torque.z, data.ft_wrench.force.x,
data.ft_wrench.force.y, data.ft_wrench.force.z
]).reshape(6, 1)
self.force_torque_data = np.concatenate(
(self.force_torque_data, incoming), axis=1)
if (self.data_count > 500):
self.force_torque_data = self.force_torque_data[..., 1:]
self.x_data = self.x_data[1:]
self.force_torque_plot_widget.setRange(
xRange=(self.x_data[1], self.x_data[-1]))
else:
self.data_count += 1
for i in range(6):
self.plot_container[i].setData(
self.x_data, self.force_torque_data[i, ...])
self.force_torque_app.processEvents()
if (not self.joint_angle_plot_widget.isHidden()):
self.x_data = np.concatenate((self.x_data, [data.header.seq]))
incoming = np.array([
data.ft_wrench.torque.x, data.ft_wrench.torque.y,
data.ft_wrench.torque.z, data.ft_wrench.force.x,
data.ft_wrench.force.y, data.ft_wrench.force.z
]).reshape(7, 1)
self.joint_angle_data = np.concatenate(
(self.joint_angle_data, incoming), axis=1)
if (self.data_count > 500):
self.joint_angle_data = self.joint_angle_data[..., 1:]
self.x_data = self.x_data[1:]
self.joint_angle_plot_widget.setRange(
xRange=(self.x_data[1], self.x_data[-1]))
else:
self.data_count += 1
for i in range(7):
self.plot_container[i].setData(
self.x_data, self.joint_angle_data[i, ...])
self.joint_angle_app.processEvents()
#if(len(self._data_array)>10):
# for x in self._data_array:
# self._widget.State_info.append(x)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def setup(self, config_layout, results_layout, config_locked):
'''
set up the layout for selecting options from the different benchmark variations
'''
config_group_box_layout = QHBoxLayout()
self.config_group_box = QGroupBox('Configuration')
self.config_group_box.setCheckable(True)
if (config_locked):
self.config_group_box.setChecked(False)
variations = self.config['variations'].keys()
for var in variations:
single_choice = True
if var in self.config['multiple choice variation']:
single_choice = False
if isinstance(self.config['variations'][var], list):
self.variation_widgets[var], scroll = self.add_config(
var,
self.config['variations'][var],
single_choice=single_choice)
config_group_box_layout.addWidget(scroll)
else:
path = os.path.join(self.config_path,
self.config['variations'][var])
if os.path.exists(path):
with open(path) as f:
options = f.readlines()
options = [x.strip() for x in options]
self.variation_widgets[var], scroll = self.add_config(
var, options, single_choice=single_choice)
config_group_box_layout.addWidget(scroll)
self.config['variations'][var] = options
else:
QMessageBox.critical(None, "Error",
"File %s does not exist" % path)
self.config_group_box.setLayout(config_group_box_layout)
config_layout.addWidget(self.config_group_box)
results_group_box = QGroupBox('Results')
results_group_box_layout = QHBoxLayout()
results = self.config['results'].keys()
for res in results:
single_choice = True
if var in self.config['multiple choice result']:
single_choice = False
if isinstance(self.config['results'][res], list):
self.result_widgets[res], scroll = self.add_config(
res,
self.config['results'][res],
single_choice=single_choice)
results_group_box_layout.addWidget(scroll)
else:
if self.config['results'][res] == 'text_field':
widget = QWidget()
text_field = QLineEdit()
text_field.setReadOnly(True)
txtlayout = QHBoxLayout()
txtlayout.addWidget(QLabel(res))
txtlayout.addWidget(text_field)
widget.setLayout(txtlayout)
self.result_widgets[res] = text_field
results_group_box_layout.addWidget(widget)
results_group_box.setLayout(results_group_box_layout)
results_layout.addWidget(results_group_box)
self.notes_widget = QPlainTextEdit()
self.notes_widget.setMaximumHeight(100)
self.notes_widget.setPlaceholderText('Enter notes about the result...')
results_layout.addWidget(self.notes_widget)
class FrameEditor_StyleWidget(Interface):
def __init__(self, frame_editor):
self.editor = frame_editor
self.editor.observers.append(self)
self.old_frame = None
self.layout = QtWidgets.QGridLayout()
self.widget = QWidget()
self.widget.setLayout(self.layout)
self.mesh_label = QtWidgets.QLineEdit("File:")
self.mesh_label.setSizePolicy(QtWidgets.QSizePolicy.Ignored,
QtWidgets.QSizePolicy.Fixed)
self.mesh_button = QtWidgets.QPushButton("Open")
self.mesh_button.clicked.connect(lambda: self.btn_open_mesh_clicked())
self.diameter_label = QtWidgets.QLabel("Diameter:")
self.diameter_spinbox = QtWidgets.QDoubleSpinBox()
self.diameter_spinbox.editingFinished.connect(
lambda: self.diameter_changed())
self.length_label = QtWidgets.QLabel("Length:")
self.length_spinbox = QtWidgets.QDoubleSpinBox()
self.length_spinbox.editingFinished.connect(
lambda: self.length_changed())
self.width_label = QtWidgets.QLabel("Width:")
self.width_spinbox = QtWidgets.QDoubleSpinBox()
self.width_spinbox.editingFinished.connect(
lambda: self.width_changed())
self.height_label = QtWidgets.QLabel("Height:")
self.height_spinbox = QtWidgets.QDoubleSpinBox()
self.height_spinbox.editingFinished.connect(
lambda: self.height_changed())
self.color_label = QtWidgets.QLabel()
self.color_label.setAutoFillBackground(True)
self.update_color_label(None)
self.color_button = QtWidgets.QPushButton("Set Color")
self.color_button.clicked.connect(lambda: self.btn_color_clicked())
self.layout.addWidget(self.mesh_label, 0, 0)
self.layout.addWidget(self.mesh_button, 0, 1)
self.layout.addWidget(self.diameter_label, 1, 0)
self.layout.addWidget(self.diameter_spinbox, 1, 1)
self.layout.addWidget(self.length_label, 2, 0)
self.layout.addWidget(self.length_spinbox, 2, 1)
self.layout.addWidget(self.width_label, 3, 0)
self.layout.addWidget(self.width_spinbox, 3, 1)
self.layout.addWidget(self.height_label, 4, 0)
self.layout.addWidget(self.height_spinbox, 4, 1)
self.layout.addWidget(self.color_label, 5, 0)
self.layout.addWidget(self.color_button, 5, 1)
print("init")
self.update_widget(None)
def get_widget(self):
return self.widget
def update(self, editor, level, elements):
if level & 2:
## Check for change
if self.editor.active_frame is not self.old_frame:
self.update_widget(self.editor.active_frame)
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
elif level & 4:
if self.editor.active_frame is not None:
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
def update_widget(self, frame):
## Clear layout
#while self.layout.count():
# child = self.layout.takeAt(0)
# child.widget().deleteLater()
self.mesh_label.hide()
self.mesh_button.hide()
self.diameter_label.hide()
self.diameter_spinbox.hide()
self.length_label.hide()
self.length_spinbox.hide()
self.width_label.hide()
self.width_spinbox.hide()
self.height_label.hide()
self.height_spinbox.hide()
if frame is None or frame.style == "none":
self.widget.setEnabled(False)
return
if frame.style == "mesh":
self.mesh_label.show()
self.mesh_button.show()
elif frame.style == "sphere":
self.diameter_label.show()
self.diameter_spinbox.show()
else:
self.length_label.show()
self.length_spinbox.show()
self.width_label.show()
self.width_spinbox.show()
if frame.style == "cube":
self.height_label.show()
self.height_spinbox.show()
self.widget.setEnabled(True)
def update_values(self, frame):
if frame is None or frame.style == "none":
return
if frame.style == "mesh":
self.mesh_label.setText(frame.path)
elif frame.style == "sphere":
self.diameter_spinbox.setValue(frame.diameter)
else:
self.length_spinbox.setValue(frame.length)
self.width_spinbox.setValue(frame.width)
if frame.style == "cube":
self.height_spinbox.setValue(frame.height)
def update_color_label(self, frame):
if frame is None:
values = "{}, {}, {}, {}".format(200, 200, 200, 255)
else:
values = "{}, {}, {}, {}".format(frame.color[0] * 255,
frame.color[1] * 255,
frame.color[2] * 255,
frame.color[3] * 255)
self.color_label.setStyleSheet("QLabel { background-color: rgba(" +
values + "); }")
@Slot(float)
def diameter_changed(self):
if self.editor.active_frame.diameter != self.diameter_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"diameter", self.diameter_spinbox.value()))
@Slot(float)
def length_changed(self):
if self.editor.active_frame.length != self.length_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"length", self.length_spinbox.value()))
@Slot(float)
def width_changed(self):
if self.editor.active_frame.width != self.width_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"width", self.width_spinbox.value()))
@Slot(float)
def height_changed(self):
if self.editor.active_frame.height != self.height_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"height", self.height_spinbox.value()))
@Slot(bool)
def btn_open_mesh_clicked(self):
path = QtWidgets.QFileDialog.getOpenFileName(None, 'Open Mesh',
'/home',
'Mesh Files (*.stl)')[0]
try:
rospackage = rospkg.get_package_name(path)
if rospackage is None:
QtWidgets.QMessageBox.warning(
self.widget, "Saving absolute path to mesh",
"Cannot find rospackage with selected mesh in it!\nSaving absolute path to mesh instead!"
)
#print("WARNING cannot find rospackage with mesh in it, saving absolute path")
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", ""))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", path))
else:
rel_path = os.path.relpath(
path,
rospkg.RosPack().get_path(rospackage))
print("Saving: package: {} + relative path: {}".format(
rospackage, rel_path))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", rospackage))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", rel_path))
except:
QtWidgets.QMessageBox.warning(
self.widget, "Saving absolute path to mesh",
"The found rospackage with selected mesh in it is not sourced in your ROS workspace!\n"
+
"Cannot resolve the packagepath\nSaving absolute path to mesh instead!"
)
#print("The package found is not sourced withing the current workspace, saving absolute path instead!")
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", ""))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", path))
@Slot(bool)
def btn_color_clicked(self):
frame = self.editor.active_frame
color = QtWidgets.QColorDialog.getColor(
QColor(frame.color[0] * 255, frame.color[1] * 255,
frame.color[2] * 255, frame.color[3] * 255),
None,
"Select Color",
options=QtWidgets.QColorDialog.ShowAlphaChannel)
self.editor.command(
Command_SetStyleColor(self.editor, frame, color.getRgbF()))
def __init__(self, context):
#Start client -rosbridge
self.client = roslibpy.Ros(host='localhost', port=5803)
self.client.run()
super(Dashboard, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Dashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_dashboard'), 'resource', 'Dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DashboardUi')
# Set up signal-slot connections
self._widget.set_imu_angle_button.clicked.connect(self.setImuAngle)
self._widget.imu_angle.valueChanged.connect(self.imuAngleChanged)
self._widget.auto_wall_dist_button.clicked.connect(self.setAutoWallDist)
self._widget.auto_wall_dist.valueChanged.connect(self.autoWallDistChanged)
self._widget.ball_reset_button.clicked.connect(self.resetBallCount)
self._widget.ball_reset_count.valueChanged.connect(self.resetBallChanged)
# Add buttons for auto modes
v_layout = self._widget.auto_mode_v_layout #vertical layout storing the buttons
self.auto_mode_button_group = QButtonGroup(self._widget) # needs to be a member variable so the publisher can access it to see which auto mode was selected
# Search for auto_mode config items
for i in range(1,100): # loop will exit when can't find the next auto mode, so really only a while loop needed, but exiting at 100 will prevent infinite looping
if rospy.has_param("/auto/auto_mode_" + str(i)):
auto_sequence = rospy.get_param("/auto/auto_mode_" + str(i))
new_auto_mode = QWidget()
new_h_layout = QHBoxLayout()
new_h_layout.setContentsMargins(0,0,0,0)
new_button = QRadioButton("Mode " + str(i))
new_button.setStyleSheet("font-weight: bold")
self.auto_mode_button_group.addButton(new_button, i) #second arg is the button's id
new_h_layout.addWidget( new_button )
new_h_layout.addWidget( QLabel(", ".join(auto_sequence)) )
hSpacer = QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Minimum)
new_h_layout.addItem(hSpacer)
new_auto_mode.setLayout( new_h_layout )
v_layout.addWidget(new_auto_mode)
else:
print(str(i-1) + " auto modes found.")
# if no auto modes found, inform the user with a label
if (i-1) == 0:
v_layout.addWidget( QLabel("No auto modes found") )
break #break out of for loop searching for auto modes
# auto state stuff
self.autoState = 0
self.displayAutoState() #display initial auto state
# publish thread
publish_thread = Thread(target=self.publish_thread) #args=(self,))
publish_thread.start()
# number balls display
self.zero_balls = QPixmap(":/images/0_balls.png")
self.one_ball = QPixmap(":/images/1_ball.png")
self.two_balls = QPixmap(":/images/2_balls.png")
self.three_balls = QPixmap(":/images/3_balls.png")
self.four_balls = QPixmap(":/images/4_balls.png")
self.five_balls = QPixmap(":/images/5_balls.png")
self.more_than_five_balls = QPixmap(":/images/more_than_5_balls.png")
self.n_balls = -1 #don't know n balls at first
#in range stuff
self.shooter_in_range = False
self.turret_in_range = False
self.in_range_pixmap = QPixmap(":/images/GreenTarget.png")
self.not_in_range_pixmap = QPixmap(":/images/RedTarget.png")
self._widget.in_range_display.setPixmap(self.not_in_range_pixmap)
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
#initialize subscribers last, so that any callbacks they execute won't interfere with init
auto_state_listener = roslibpy.Topic(self.client, '/auto/auto_state', 'behavior_actions/AutoState')
self.auto_state_sub = auto_state_listener.subscribe(self.autoStateCallback)
n_balls_listener = roslibpy.Topic(self.client,'/num_powercells','std_msgs/UInt8')
self.n_balls_sub = n_balls_listener.subscribe(self.nBallsCallback)
shooter_in_range_listener = roslibpy.Topic(self.client, '/shooter/shooter_in_range', std_msgs.msg.Bool)
self.shooter_in_range_sub = shooter_in_range_listener.subscribe(self.shooterInRangeCallback)
turret_in_range_listener = roslibpy.Topic(self.client, '/align_to_shoot/turret_in_range', std_msgs.msg.Bool)
self.turret_in_range_sub = turret_in_range_listener.subscribe(self.turretInRangeCallback)
self.autoStateSignal.connect(self.autoStateSlot)
self.nBallsSignal.connect(self.nBallsSlot)
self.shooterInRangeSignal.connect(self.shooterInRangeSlot)
self.turretInRangeSignal.connect(self.turretInRangeSlot)
class BagParser(QWidget):
def __init__(self, bag_files, listtopics, duration):
super(BagParser, self).__init__()
# window title
self.setWindowTitle("Making csv file")
# size of window
self.resize(960, 720)
#self.showFullScreen()
#self.setWindowState(Qt.WindowMaximized)
# print listtopics
# print E.get_general_features_options()
# print E.get_specific_features_options()
self.topics_items = dict()
self.topics_items["0"] = listtopics
self.topics_items["1"] = E.get_general_features_options()
self.topics_items["2"] = E.get_specific_features_options()
print self.topics_items
#path to bag file
self.bag_files = bag_files
self.selected_bag_topics = []
self.selected_specific_features = []
self.selected_general_features = []
self.items_list_topics = []
self.area = QScrollArea(self)
self.areagen = QScrollArea(self)
self.areaspec = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.main_widget1 = QWidget(self.areagen)
self.main_widget2 = QWidget(self.areaspec)
self.ok_button = QPushButton("Export To CSV", self)
#self.ok_button.setFixedSize(150, 30)
self.ok_button.clicked.connect(self.onButtonClicked)
self.clear_button = QPushButton("Clear Selection", self)
# self.clear_button.resize(self.clear_button.sizeHint())
self.clear_button.clicked.connect(self.onClearClicked)
self.choose_button = QPushButton("Get Last Export Choose", self)
self.choose_button.clicked.connect(self.onButtonChooseCliked)
self.ok_button.setEnabled(False)
self.label1 = QLabel("Select topic from bag(s)", self)
self.label1.setAlignment(Qt.AlignCenter)
self.label2 = QLabel("Statistics Features", self)
self.label2.setAlignment(Qt.AlignCenter)
self.label3 = QLabel("Specific Features", self)
self.label3.setAlignment(Qt.AlignCenter)
self.duration = duration
self.label5 = QLabel("Duration Time: " + str("%.1f" % duration), self)
self.label5.setAlignment(Qt.AlignCenter)
self.main_vlayout = QVBoxLayout(self)
# self.main_vlayout = QGridLayout(self)
self.main_vlayout.addWidget(self.label1)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.label2)
self.main_vlayout.addWidget(self.areagen)
self.main_vlayout.addWidget(self.label3)
self.main_vlayout.addWidget(self.areaspec)
self.label4 = QLabel("Window time", self)
self.label4.setAlignment(Qt.AlignCenter)
# self.main_vlayout.addWidget(self.label4)
self.window = QLineEdit(self)
# self.main_vlayout.addWidget(self.window)
self.window.setText("1")
self.windows_time_3 = QHBoxLayout(self)
self.windows_time_3.addWidget(self.label4)
self.windows_time_3.addWidget(self.window)
self.windows_time_3.addWidget(self.label5)
self.main_vlayout.addLayout(self.windows_time_3)
# self.window = QLineEdit(self)
# self.window.setText("1")
# self.box = QVBoxLayout()
# self.box.addStretch(1)
# self.box.addWidget(self.clear_button)
# self.box.addWidget(self.choose_button)
# self.box.addWidget(self.label4)
# self.box.addWidget(self.window)
# self.box.addWidget(self.label5)
# self.box.addWidget(self.ok_button)
#self.main_vlayout.addWidget(self.from_nodes_button)
# self.main_vlayout.addLayout(self.box)
self.two_buttons = QHBoxLayout(self)
self.two_buttons.addWidget(self.choose_button)
self.two_buttons.addWidget(self.clear_button)
self.main_vlayout.addLayout(self.two_buttons)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = MyQCheckBox("All", self, self.selection_vlayout, None)
self.item_all.stateChanged.connect(
lambda x: self.updateList(x, self.item_all, None))
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = listtopics
topic_data_list.sort()
for topic in topic_data_list:
self.addCheckBox(topic, self.selection_vlayout,
self.selected_bag_topics)
self.selection_vlayout1 = QVBoxLayout(self)
self.item_all1 = MyQCheckBox("All", self, self.selection_vlayout1,
None)
self.item_all1.stateChanged.connect(
lambda x: self.updateList(x, self.item_all1, None))
self.selection_vlayout1.addWidget(self.item_all1)
topic_data_list1 = E.get_general_features_options()
topic_data_list1.sort()
for topic in topic_data_list1:
self.addCheckBox(topic, self.selection_vlayout1,
self.selected_general_features)
self.selection_vlayout2 = QVBoxLayout(self)
self.item_all2 = MyQCheckBox("All", self, self.selection_vlayout2,
None)
self.item_all2.stateChanged.connect(
lambda x: self.updateList(x, self.item_all2, None))
self.selection_vlayout2.addWidget(self.item_all2)
topic_data_list2 = E.get_specific_features_options()
topic_data_list2.sort()
for topic in topic_data_list2:
self.addCheckBox(topic, self.selection_vlayout2,
self.selected_specific_features)
self.main_widget.setLayout(self.selection_vlayout)
self.main_widget1.setLayout(self.selection_vlayout1)
self.main_widget2.setLayout(self.selection_vlayout2)
self.area.setWidget(self.main_widget)
self.areagen.setWidget(self.main_widget1)
self.areaspec.setWidget(self.main_widget2)
self.show()
def onClearClicked(self):
self.clearTopicCheckState()
def clearTopicCheckState(self):
for item in self.items_list_topics:
item.setCheckState(False)
self.item_all.setCheckState(False)
self.item_all1.setCheckState(False)
self.item_all2.setCheckState(False)
def addCheckBox(self, topic, selection_vlayout, selected_list):
item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
self.items_list_topics.append(item)
selection_vlayout.addWidget(item)
def changeTopicCheckState(self, topic, state):
for item in self.items_list_topics:
if item.text() == topic:
item.setCheckState(state)
return
def updateList(self,
state,
item_clicked,
topic=None,
force_update_state=False):
if topic is None: # The "All" checkbox was checked / unchecked
# print "if topic is None"
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list_topics:
if item.checkState() == Qt.Unchecked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list_topics:
if item.checkState() == Qt.Checked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Unchecked)
else:
# print "else:"
if state == Qt.Checked:
item_clicked.selected_list.append(topic)
print item_clicked.selected_list
else:
item_clicked.selected_list.remove(topic)
#if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_specific_features != []:
if self.selected_general_features != [] and self.selected_bag_topics != []:
self.ok_button.setEnabled(True)
elif self.selected_general_features == [] and self.selected_bag_topics == []:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
else:
if self.selected_general_features != [] and self.selected_bag_topics != []:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
# if self.selected_bag_topics != []:
# if self.selected_specific_features == [] and self.selected_general_features == []:
# self.ok_button.setEnabled(False)
# else:
# self.ok_button.setEnabled(True)
# # elif self.selected_specific_features != [] and self.selected_general_features == []:
# # self.ok_button.setEnabled(True)
# # elif self.selected_specific_features == [] and self.selected_general_features != []:
# # self.ok_button.setEnabled(True)
# else:
# self.ok_button.setEnabled(False)
# if self.selected_specific_features != []:
# if self.selected_bag_topics == [] and self.selected_general_features == []:
# self.ok_button.setEnabled(True)
# elif self.selected_bag_topics != [] and self.selected_general_features != []:
# self.ok_button.setEnabled(True)
# else:
# self.ok_button.setEnabled(False)
# else:
# if self.selected_bag_topics == [] or self.selected_general_features == []:
# self.ok_button.setEnabled(False)
# else:
# self.ok_button.setEnabled(True)
def onButtonChooseCliked(self):
for checkbox in self.items_list_topics:
checkbox.setCheckState(Qt.Unchecked)
with open(get_path() + "logger.log", 'r') as f:
topics = f.read().splitlines()
for checkbox in self.items_list_topics:
if checkbox.text() in topics:
checkbox.setCheckState(Qt.Checked)
def get_current_opened_directory(self, filepath):
import os
direc = "/"
if os.path.isfile(filepath):
with open(filepath, 'r') as f:
direc = f.read()
return direc
def onButtonClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/save_csv.log"
current_directory = self.get_current_opened_directory(filepath)
window = self.window.text()
try:
val = float(window)
except ValueError:
QMessageBox.about(self, "Error in Window Time",
"That's not a number!")
return
if val >= self.duration:
QMessageBox.about(
self, "Error in Window Time",
"time need to be smaller than: " + str(self.duration))
return
# filename = QFileDialog.getSaveFileName(self, self.tr('csv File'), current_directory, self.tr('csv (*.csv)'))
saved_dir = str(
QFileDialog.getExistingDirectory(self, "Select Directory",
current_directory))
# if filename[0] != '':
# with open(filepath, "w") as f:
# f.write(filename[0])
if saved_dir != '':
with open(filepath, "w") as f:
f.write(saved_dir)
topics = self.selected_bag_topics
specific_features_selection = self.selected_specific_features
general_features_selection = self.selected_general_features
with open(get_path() + 'logger.log', "w") as f:
for topic in topics:
f.write(topic + "\n")
for topic1 in specific_features_selection:
f.write(topic1 + "\n")
for topic2 in general_features_selection:
f.write(topic2 + "\n")
ef = E.ExtractFeatures(topics, float(window),
specific_features_selection,
general_features_selection)
counter = 0
for bag_file in self.bag_files:
df = ef.generate_features(bag_file)
if len(self.bag_files) == 1:
counter = -1
# temp = filename + "/" +
# temp = get_corrent_file_name(filename[0], ".csv", counter)
csv_path = generate_csv_from_bag(saved_dir, bag_file)
# temp = "%s_%s%s" % (filename[0],counter,".csv")
E.write_to_csv(csv_path, df)
counter = counter + 1
QMessageBox.about(self, "csv export",
"csv was exported successfuly")
class FrameEditor_StyleWidget(Interface):
def __init__(self, frame_editor):
self.editor = frame_editor
self.editor.observers.append(self)
self.old_frame = None
self.layout = QtWidgets.QGridLayout()
self.widget = QWidget()
self.widget.setLayout(self.layout)
self.mesh_label = QtWidgets.QLineEdit("File:")
self.mesh_label.setSizePolicy(QtWidgets.QSizePolicy.Ignored,
QtWidgets.QSizePolicy.Fixed)
self.mesh_button = QtWidgets.QPushButton("Open")
self.mesh_button.clicked.connect(self.btn_open_mesh_clicked)
self.diameter_label = QtWidgets.QLabel("Diameter:")
self.diameter_spinbox = QtWidgets.QDoubleSpinBox()
self.diameter_spinbox.editingFinished.connect(self.diameter_changed)
self.length_label = QtWidgets.QLabel("Length:")
self.length_spinbox = QtWidgets.QDoubleSpinBox()
self.length_spinbox.editingFinished.connect(self.length_changed)
self.width_label = QtWidgets.QLabel("Width:")
self.width_spinbox = QtWidgets.QDoubleSpinBox()
self.width_spinbox.editingFinished.connect(self.width_changed)
self.height_label = QtWidgets.QLabel("Height:")
self.height_spinbox = QtWidgets.QDoubleSpinBox()
self.height_spinbox.editingFinished.connect(self.height_changed)
self.color_label = QtWidgets.QLabel()
self.color_label.setAutoFillBackground(True)
self.update_color_label(None)
self.color_button = QtWidgets.QPushButton("Set Color")
self.color_button.clicked.connect(self.btn_color_clicked)
self.layout.addWidget(self.mesh_label, 0, 0)
self.layout.addWidget(self.mesh_button, 0, 1)
self.layout.addWidget(self.diameter_label, 1, 0)
self.layout.addWidget(self.diameter_spinbox, 1, 1)
self.layout.addWidget(self.length_label, 2, 0)
self.layout.addWidget(self.length_spinbox, 2, 1)
self.layout.addWidget(self.width_label, 3, 0)
self.layout.addWidget(self.width_spinbox, 3, 1)
self.layout.addWidget(self.height_label, 4, 0)
self.layout.addWidget(self.height_spinbox, 4, 1)
self.layout.addWidget(self.color_label, 5, 0)
self.layout.addWidget(self.color_button, 5, 1)
self.update_widget(None)
def get_widget(self):
return self.widget
def update(self, editor, level, elements):
if level & 2:
## Check for change
if self.editor.active_frame is not self.old_frame:
self.update_widget(self.editor.active_frame)
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
elif level & 4:
if self.editor.active_frame is not None:
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
def update_widget(self, frame):
## Clear layout
#while self.layout.count():
# child = self.layout.takeAt(0)
# child.widget().deleteLater()
self.mesh_label.hide()
self.mesh_button.hide()
self.diameter_label.hide()
self.diameter_spinbox.hide()
self.length_label.hide()
self.length_spinbox.hide()
self.width_label.hide()
self.width_spinbox.hide()
self.height_label.hide()
self.height_spinbox.hide()
if frame is None or frame.style == "none":
self.widget.setEnabled(False)
return
if frame.style == "mesh":
self.mesh_label.show()
self.mesh_button.show()
elif frame.style == "sphere":
self.diameter_label.show()
self.diameter_spinbox.show()
else:
self.length_label.show()
self.length_spinbox.show()
self.width_label.show()
self.width_spinbox.show()
if frame.style == "cube":
self.height_label.show()
self.height_spinbox.show()
self.widget.setEnabled(True)
def update_values(self, frame):
if frame is None or frame.style == "none":
return
if frame.style == "mesh":
self.mesh_label.setText(frame.path)
elif frame.style == "sphere":
self.diameter_spinbox.setValue(frame.diameter)
else:
self.length_spinbox.setValue(frame.length)
self.width_spinbox.setValue(frame.width)
if frame.style == "cube":
self.height_spinbox.setValue(frame.height)
def update_color_label(self, frame):
if frame is None:
values = "{}, {}, {}, {}".format(200, 200, 200, 255)
else:
values = "{}, {}, {}, {}".format(frame.color[0] * 255,
frame.color[1] * 255,
frame.color[2] * 255,
frame.color[3] * 255)
self.color_label.setStyleSheet("QLabel { background-color: rgba(" +
values + "); }")
@Slot(float)
def diameter_changed(self):
if self.editor.active_frame.diameter != self.diameter_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"diameter", self.diameter_spinbox.value()))
@Slot(float)
def length_changed(self):
if self.editor.active_frame.length != self.length_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"length", self.length_spinbox.value()))
@Slot(float)
def width_changed(self):
if self.editor.active_frame.width != self.width_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"width", self.width_spinbox.value()))
@Slot(float)
def height_changed(self):
if self.editor.active_frame.height != self.height_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"height", self.height_spinbox.value()))
@Slot()
def btn_open_mesh_clicked(self):
path = QtWidgets.QFileDialog.getOpenFileName(
None, 'Open Mesh', '/home', 'Mesh Files (*.stl *.dae)')[0]
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame, "path",
path))
@Slot(bool)
def btn_color_clicked(self, checked):
frame = self.editor.active_frame
color = QtWidgets.QColorDialog.getColor(
QtWidgets.QColor(frame.color[0] * 255, frame.color[1] * 255,
frame.color[2] * 255, frame.color[3] * 255),
None,
"Select Color",
options=QtWidgets.QColorDialog.ShowAlphaChannel)
self.editor.command(
Command_SetStyleColor(self.editor, frame, color.getRgbF()))
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def onValueChangedOne(self, name):
# A slider value was changed, but we need to change the joint_info metadata.
joint_info = self.joint_map[name]
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'],
joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(
joint['position'], joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items),
self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows == 0:
return []
positions = [
(y, x)
for x in range(int((math.ceil(float(num_items) / num_rows))))
for y in range(num_rows)
]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']),
joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] -
joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max'] - joint['min']) * pctvalue
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_all.stateChanged.connect(self.updateList)
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x, topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def updateList(self, state, topic=None):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState() == Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit(
(self.item_all.checkState() == Qt.Checked), self.selected_topics)
