def __init__(self, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = '/home/mohammad/Documents/CMU/RED/catkin_ws/src/rqt_safeguard/resource/plot.ui'
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
# self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
# self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
self.color_box.setStyleSheet("background-color: red;")
# init and start update timer for plot
self._update_timer = QTimer(self)
self._update_timer.timeout.connect(self.update)
def __init__(self, arguments=None, initial_topics=None):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._ordered_topics = []
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
# save command line arguments
self._arguments = arguments
self._initial_topics = initial_topics
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName('Plot2DWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
def __init__(self, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_histogram'), 'resource',
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
def __init__(self, node, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._node = node
self._initial_topics = initial_topics
_, package_path = get_resource('packages', 'rqt_plot')
ui_file = os.path.join(package_path, 'share', 'rqt_plot', 'resource',
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics(node)
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = time.time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
def __init__(self, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
def __init__(self,
initial_topics=None,
start_paused=False,
buffer_length=100,
use_poly=True,
no_legend=False):
super(Plot3DWidget, self).__init__()
self.setObjectName('Plot3DWidget')
self._buffer_length = buffer_length
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot3d.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatDataPlot3D(self, self._buffer_length, use_poly,
no_legend)
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
def __init__(self):
super(MatPlotWidget, self).__init__()
self.setObjectName("MatPlotWidget")
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), "MatPlot.ui")
loadUi(ui_file, self, {"MatDataPlot": MatDataPlot})
self.subscribe_topic_button.setEnabled(False)
self._topic_completer = TopicCompleter(self.topic_edit)
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
# setup drag 'n drop
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(40)
# connect combobox
self.comboBox.currentIndexChanged.connect(self.on_combo_box_changed)
# params (colors)
self.texts = {}
self.data_plot._colors = {}
self.sub_topics = {}
n = 0
for tag in rospy.get_param("/tags"):
self.texts[tag] = rospy.get_param("/" + tag + "_text")
color = [float(i) for i in list(rospy.get_param("/" + tag + "_color").split(","))]
self.data_plot._colors[self.texts[tag]] = color
if n != 0: # dont have a topic for the reference
self.sub_topics[tag] = "/error_mags/" + rospy.get_param("/tags")[0] + "_ref/" + tag + "_tgt/mag_horiz"
else:
n += 1
# start with subscription to filtered
self.add_topic("/error_mags/rtk_ref/flt_tgt/mag_horiz")
def __init__(self, initial_topics=None, start_paused=False,
buffer_length=100, use_poly=True, no_legend=False):
super(Plot3DWidget, self).__init__()
self.setObjectName('Plot3DWidget')
self._buffer_length = buffer_length
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot3d.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatDataPlot3D(self, self._buffer_length,
use_poly, no_legend)
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
class MatPlotWidget(QWidget):
"""The actual Widget """
def __init__(self):
super(MatPlotWidget, self).__init__()
self.setObjectName("MatPlotWidget")
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), "MatPlot.ui")
loadUi(ui_file, self, {"MatDataPlot": MatDataPlot})
self.subscribe_topic_button.setEnabled(False)
self._topic_completer = TopicCompleter(self.topic_edit)
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
# setup drag 'n drop
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(40)
# connect combobox
self.comboBox.currentIndexChanged.connect(self.on_combo_box_changed)
# params (colors)
self.texts = {}
self.data_plot._colors = {}
self.sub_topics = {}
n = 0
for tag in rospy.get_param("/tags"):
self.texts[tag] = rospy.get_param("/" + tag + "_text")
color = [float(i) for i in list(rospy.get_param("/" + tag + "_color").split(","))]
self.data_plot._colors[self.texts[tag]] = color
if n != 0: # dont have a topic for the reference
self.sub_topics[tag] = "/error_mags/" + rospy.get_param("/tags")[0] + "_ref/" + tag + "_tgt/mag_horiz"
else:
n += 1
# start with subscription to filtered
self.add_topic("/error_mags/rtk_ref/flt_tgt/mag_horiz")
def update_plot(self):
for topic_name, rosdata in self._rosdata.items():
data_x, data_y = rosdata.next()
self.data_plot.update_value(topic_name, data_x, data_y)
self.data_plot.draw_plot()
@Slot("QDragEnterEvent*")
def dragEnterEvent(self, event):
if not event.mimeData().hasText():
if not hasattr(event.source(), "selectedItems") or len(event.source().selectedItems()) == 0:
qWarning(
"MatPlot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0"
)
return
item = event.source().selectedItems()[0]
ros_topic_name = item.data(0, Qt.UserRole)
if ros_topic_name == None:
qWarning("MatPlot.dragEnterEvent(): not hasattr(item, ros_topic_name_)")
return
# get topic name
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
# check for numeric field type
is_numeric, message = is_slot_numeric(topic_name)
if is_numeric:
event.acceptProposedAction()
else:
qWarning('MatPlot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot("QDropEvent*")
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ("", "/"):
self._topic_completer.update_topics()
is_numeric, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning("MatPlot.add_topic(): topic already subscribed: %s" % topic_name)
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
data_x, data_y = self._rosdata[topic_name].next()
color = self.data_plot._colors[self.comboBox.currentText()]
self.data_plot.add_curve(topic_name, data_x, data_y, color)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
@Slot(bool)
def on_pause_button_clicked(self, checked):
if checked:
self._update_plot_timer.stop()
else:
self._update_plot_timer.start(40)
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
@Slot(str)
def on_combo_box_changed(self, text):
# print('In on_combo_box_changed')
self.on_clear_button_clicked()
self.data_plot.tgt_name = self.comboBox.currentText()
tag = [tg for tg, txt in self.texts.iteritems() if txt == self.comboBox.currentText()][0]
self.add_topic(self.sub_topics[tag])
window_title = " ".join(
[
"Ground Plane Error Magnitude of Sensor:",
self.comboBox.currentText(),
"for Reference:",
self.data_plot.ref_name,
]
)
self.setWindowTitle(QApplication.translate("MatPlotWidget", window_title, None, QApplication.UnicodeUTF8))
class PlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, node, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._node = node
self._initial_topics = initial_topics
_, package_path = get_resource('packages', 'rqt_plot')
ui_file = os.path.join(package_path, 'share', 'rqt_plot', 'resource',
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics(node)
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = time.time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
def switch_data_plot_widget(self, data_plot):
self.enable_timer(enabled=False)
self.data_plot_layout.removeWidget(self.data_plot)
if self.data_plot is not None:
self.data_plot.close()
self.data_plot = data_plot
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
# setup drag 'n drop
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
else:
for topic_name, rosdata in self._rosdata.items():
data_x, data_y = rosdata.next()
self.data_plot.add_curve(topic_name, topic_name, data_x,
data_y)
self._subscribed_topics_changed()
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or \
len(event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or '
'len(event.source().selectedItems()) == 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
)
return
else:
topic_name = str(event.mimeData().text())
# check for plottable field type
plottable, message = is_plottable(self._node, topic_name)
if plottable:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics(self._node)
plottable, message = is_plottable(self._node, topic_name)
self.subscribe_topic_button.setEnabled(plottable)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot(bool)
def on_autoscroll_checkbox_clicked(self, checked):
self.data_plot.autoscroll(checked)
if checked:
self.data_plot.redraw()
@Slot()
def on_clear_button_clicked(self):
self.clear_plot()
def update_plot(self):
if self.data_plot is not None:
needs_redraw = False
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
if data_x or data_y:
self.data_plot.update_values(topic_name, data_x,
data_y)
needs_redraw = True
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' %
e)
if needs_redraw:
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer(self._rosdata)
self.data_plot.redraw()
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
if len(self._rosdata) > 1:
all_action = QAction('All', self._remove_topic_menu)
all_action.triggered.connect(self.clean_up_subscribers)
self._remove_topic_menu.addAction(all_action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
topics_changed = False
for topic_name in get_plot_fields(self._node, topic_name)[0]:
if topic_name in self._rosdata:
qWarning(
'PlotWidget.add_topic(): topic already subscribed: %s' %
topic_name)
continue
self._rosdata[topic_name] = ROSData(self._node, topic_name,
self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
else:
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x,
data_y)
topics_changed = True
if topics_changed:
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clear_plot(self):
for topic_name, _ in self._rosdata.items():
self.data_plot.clear_values(topic_name)
self.data_plot.redraw()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class HistogramPlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot_histogram.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
print "hello"
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
xs = data_y[-1]
axes = self.data_plot._canvas.axes
axes.cla()
axes.set_xlim(xmin=0, xmax=len(xs))
pos = np.arange(len(xs))
#axes.xticks(range(5))
for p, x in zip(pos, xs):
axes.bar(p, x, color='r', align='center')
self.data_plot._canvas.draw()
class Plot2DWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName('Plot2DWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image",
Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
# matplotlib
# concatenate x and y in order to sort
concatenated_data = zip(data_y[-1].xs, data_y[-1].ys)
if self.sort_x:
concatenated_data.sort(key=lambda x: x[0])
xs = [d[0] for d in concatenated_data]
ys = [d[1] for d in concatenated_data]
if self.is_line:
axes.plot(xs, ys, label=self.topic_with_field_name)
else:
axes.scatter(xs, ys)
# set limit
axes.set_xlim(min(xs), max(xs))
axes.set_ylim(min(ys), max(ys))
# line fitting
if self.fit_line:
X = np.array(data_y[-1].xs)
Y = np.array(data_y[-1].ys)
A = np.array([X, np.ones(len(X))])
A = A.T
a, b = np.linalg.lstsq(A, Y)[0]
axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b))
if self.fit_line_ransac:
model_ransac = linear_model.RANSACRegressor(
linear_model.LinearRegression(),
min_samples=2,
residual_threshold=self.fit_line_ransac_outlier)
X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1))
Y = np.array(data_y[-1].ys)
model_ransac.fit(X, Y)
line_X = X
line_y_ransac = model_ransac.predict(line_X)
axes.plot(line_X,
line_y_ransac,
"r--",
label="{0} x + {1}".format(
model_ransac.estimator_.coef_[0][0],
model_ransac.estimator_.intercept_[0]))
if not self.no_legend:
axes.legend(prop={'size': '8'})
axes.grid()
if self.xtitle:
axes.set_xlabel(self.xtitle)
if self.ytitle:
axes.set_ylabel(self.ytitle)
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class HistogramPlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot_histogram.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
print "hello"
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
xs = data_y[-1]
axes = self.data_plot._canvas.axes
axes.cla()
axes.set_xlim(xmin=0, xmax=len(xs))
pos = np.arange(len(xs))
#axes.xticks(range(5))
for p, x in zip(pos, xs):
axes.bar(p, x, color='r', align='center')
self.data_plot._canvas.draw()
class PlotWidget(QWidget):
_interval = 500
def __init__(self, initial_topics=None, start_paused=False):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = '/home/mohammad/Documents/CMU/RED/catkin_ws/src/rqt_safeguard/resource/plot.ui'
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
# self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
# self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
self.color_box.setStyleSheet("background-color: red;")
# init and start update timer for plot
self._update_timer = QTimer(self)
self._update_timer.timeout.connect(self.update)
# def switch_data_plot_widget(self, data_plot):
# self.enable_timer(enabled=False)
# self.data_plot_layout.removeWidget(self.data_plot)
# if self.data_plot is not None:
# self.data_plot.close()
# self.data_plot = data_plot
# self.data_plot_layout.addWidget(self.data_plot)
# self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
# # setup drag 'n drop
# self.data_plot.dropEvent = self.dropEvent
# self.data_plot.dragEnterEvent = self.dragEnterEvent
# if self._initial_topics:
# for topic_name in self._initial_topics:
# self.add_topic(topic_name)
# self._initial_topics = None
# else:
# for topic_name, rosdata in self._rosdata.items():
# data_x, data_y = rosdata.next()
# self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
# self._subscribed_topics_changed()
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(
event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
)
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
)
return
else:
topic_name = str(event.mimeData().text())
# check for plottable field type
plottable, message = is_plottable(topic_name)
if plottable:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
plottable, message = is_plottable(topic_name)
self.subscribe_topic_button.setEnabled(plottable)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
print("pause")
@Slot(bool)
def on_autoscroll_checkbox_clicked(self, checked):
# self.data_plot.autoscroll(checked)
if checked:
# self.data_plot.redraw()
print("checked is clicked")
# @Slot()
# def on_clear_button_clicked(self):
# print("Clear Button is clicked")
# #self.clear_plot()
def update(self):
print("update")
for topic_name, rosdata in self._rosdata.items():
decision, oriX = rosdata.next_dx()
if oriX:
self.oriX.setText(str(round(oriX, 2)))
oriY, oriZ = rosdata.next_yz()
if oriY:
self.oriY.setText(str(round(oriY, 2)))
if oriZ:
self.oriZ.setText(str(round(oriZ, 2)))
placX, placY = rosdata.next_pxy()
if placX:
self.placX.setText(str(round(100 * placX, 2)))
if placY:
self.placY.setText(str(round(100 * placY, 2)))
placZ = rosdata.next_pz()
if placZ:
self.placZ.setText(str(round(100 * placZ, 2)))
if decision:
print(decision)
self.color_box.setStyleSheet(
"background-color: red; color: white; font: bold; font-size: 96px;"
)
self.color_box.setText("Not Safe")
else:
print(decision)
self.color_box.setStyleSheet(
"background-color: green; color: white; font: bold; font-size: 96px;"
)
self.color_box.setText("Safe")
# if self.data_plot is not None:
# needs_redraw = False
# for topic_name, rosdata in self._rosdata.items():
# try:
# data_x, data_y = rosdata.next()
# if data_x or data_y:
# self.data_plot.update_values(topic_name, data_x, data_y)
# needs_redraw = True
# except RosPlotException as e:
# qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e)
# if needs_redraw:
# self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer()
print("if not self.pause_button.isChecked(): IN WIDGET PY")
#self.data_plot.redraw()
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
if len(self._rosdata) > 1:
all_action = QAction('All', self._remove_topic_menu)
all_action.triggered.connect(self.clean_up_subscribers)
self._remove_topic_menu.addAction(all_action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
self.enable_timer()
topics_changed = False
for topic_name in get_plot_fields(topic_name)[0]:
if topic_name in self._rosdata:
qWarning(
'PlotWidget.add_topic(): topic already subscribed: %s' %
topic_name)
continue
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
else:
# data_x, data_y = self._rosdata[topic_name].next() INJA TAGHIR BEDE UPDATE KON MAGHADIR RO
#self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
topics_changed = True
if topics_changed:
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
# self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
# def clear_plot(self):
# for topic_name, _ in self._rosdata.items():
# self.data_plot.clear_values(topic_name)
# self.data_plot.redraw()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
# self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_timer.start(self._interval)
else:
self._update_timer.stop()
def alaki(self):
print("alaki")
class Plot2DWidget(QWidget):
_redraw_interval = 10
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName('Plot2DWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(
rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
"callback function when form is entered"
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
rospy.loginfo("subscribe topic")
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(
topic_name + "/plot_image", Image, queue_size=1)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def plot_one(self, msg, axes):
concatenated_data = zip(msg.xs, msg.ys)
if self.sort_x:
concatenated_data.sort(key=lambda x: x[0])
xs = [d[0] for d in concatenated_data]
ys = [d[1] for d in concatenated_data]
if self.is_line or msg.type is PlotData.LINE:
axes.plot(xs, ys,
label=msg.label or self.topic_with_field_name)
else:
axes.scatter(xs, ys,
label=msg.label or self.topic_with_field_name)
if msg.fit_line or self.fit_line:
X = np.array(msg.xs)
Y = np.array(msg.ys)
A = np.array([X, np.ones(len(X))])
A = A.T
a, b = np.linalg.lstsq(A, Y, rcond=-1)[0]
axes.plot(X, (a*X+b), "g--", label="{0} x + {1}".format(a, b))
if msg.fit_line_ransac or self.fit_line_ransac:
model_ransac = linear_model.RANSACRegressor(
linear_model.LinearRegression(), min_samples=2,
residual_threshold=self.fit_line_ransac_outlier)
X = np.array(msg.xs).reshape((len(msg.xs), 1))
Y = np.array(msg.ys)
model_ransac.fit(X, Y)
line_X = X
line_y_ransac = model_ransac.predict(line_X)
if len(model_ransac.estimator_.coef_) == 1:
coef = model_ransac.estimator_.coef_[0]
else:
coef = model_ransac.estimator_.coef_[0][0]
if not isinstance(model_ransac.estimator_.intercept_, list):
intercept = model_ransac.estimator_.intercept_
else:
intercept = model_ransac.estimator_.intercept_[0]
axes.plot(
line_X, line_y_ransac, "r--",
label="{0} x + {1}".format(coef, intercept))
def update_plot(self):
if not self._rosdata:
return
try:
data_x, data_y = self._rosdata.next()
except RosPlotException as e:
rospy.logerr("Exception in subscribing topic")
rospy.logerr(e.message)
return
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
# matplotlib
# concatenate x and y in order to sort
latest_msg = data_y[-1]
min_x = None
max_x = None
min_y = None
max_y = None
if isinstance(latest_msg, PlotData):
data = [latest_msg]
legend_size = 8
no_legend = False
elif isinstance(latest_msg, PlotDataArray):
data = latest_msg.data
legend_size = latest_msg.legend_font_size or 8
no_legend = latest_msg.no_legend
if latest_msg.min_x != latest_msg.max_x:
min_x = latest_msg.min_x
max_x = latest_msg.max_x
if latest_msg.min_y != latest_msg.max_y:
min_y = latest_msg.min_y
max_y = latest_msg.max_y
else:
rospy.logerr(
"Topic should be jsk_recognition_msgs/PlotData",
"or jsk_recognition_msgs/PlotDataArray")
for d in data:
self.plot_one(d, axes)
xs = add_list([d.xs for d in data])
ys = add_list([d.ys for d in data])
if min_x is None:
min_x = min(xs)
if max_x is None:
max_x = max(xs)
if min_y is None:
min_y = min(ys)
if max_y is None:
max_y = max(ys)
axes.set_xlim(min_x, max_x)
axes.set_ylim(min_y, max_y)
# line fitting
if not no_legend and not self.no_legend:
axes.legend(prop={'size': legend_size})
axes.grid()
if self.xtitle:
axes.set_xlabel(self.xtitle)
if self.ytitle:
axes.set_ylabel(self.ytitle)
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
if LooseVersion(cv2.__version__).version[0] < 2:
iscolor = cv2.CV_LOAD_IMAGE_COLOR
else:
iscolor = cv2.IMREAD_COLOR
img = cv2.imdecode(img_array, iscolor)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class HistogramPlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image",
Image,
queue_size=1)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
if isinstance(data_y[-1], HistogramWithRange):
xs = [y.count for y in data_y[-1].bins]
pos = [y.min_value for y in data_y[-1].bins]
widths = [y.max_value - y.min_value for y in data_y[-1].bins]
axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1])
elif isinstance(data_y[-1], collections.Sequence):
xs = data_y[-1]
pos = np.arange(len(xs))
widths = [1] * len(xs)
axes.set_xlim(xmin=0, xmax=len(xs))
else:
rospy.logerr(
"Topic/Field name '%s' has unsupported '%s' type."
"List of float values and "
"jsk_recognition_msgs/HistogramWithRange are supported." %
(self.topic_with_field_name, self._rosdata.sub.data_class))
return
# axes.xticks(range(5))
for p, x, w in zip(pos, xs, widths):
axes.bar(p, x, color='r', align='center', width=w)
axes.legend([self.topic_with_field_name], prop={'size': '8'})
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
if LooseVersion(cv2.__version__).version[0] < 3:
iscolor = cv2.CV_LOAD_IMAGE_COLOR
else:
iscolor = cv2.IMREAD_COLOR
img = cv2.imdecode(img_array, iscolor)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Plot2DWidget(QWidget):
_redraw_interval = 10
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName('Plot2DWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
"callback function when form is entered"
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
rospy.loginfo("subscribe topic")
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/plot_image", Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def plot_one(self, msg, axes):
concatenated_data = zip(msg.xs, msg.ys)
if self.sort_x:
concatenated_data.sort(key=lambda x: x[0])
xs = [d[0] for d in concatenated_data]
ys = [d[1] for d in concatenated_data]
if self.is_line or msg.type is PlotData.LINE:
axes.plot(xs, ys,
label=msg.label or self.topic_with_field_name)
else:
axes.scatter(xs, ys,
label=msg.label or self.topic_with_field_name)
if msg.fit_line or self.fit_line:
X = np.array(msg.xs)
Y = np.array(msg.ys)
A = np.array([X,np.ones(len(X))])
A = A.T
a,b = np.linalg.lstsq(A,Y)[0]
axes.plot(X,(a*X+b),"g--", label="{0} x + {1}".format(a, b))
if msg.fit_line_ransac or self.fit_line_ransac:
model_ransac = linear_model.RANSACRegressor(
linear_model.LinearRegression(), min_samples=2,
residual_threshold=self.fit_line_ransac_outlier)
X = np.array(msg.xs).reshape((len(msg.xs), 1))
Y = np.array(msg.ys)
model_ransac.fit(X, Y)
line_X = X
line_y_ransac = model_ransac.predict(line_X)
axes.plot(line_X, line_y_ransac, "r--",
label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0],
model_ransac.estimator_.intercept_[0]))
def update_plot(self):
if not self._rosdata:
return
try:
data_x, data_y = self._rosdata.next()
except RosPlotException, e:
rospy.logerr("Exception in subscribing topic")
rospy.logerr(e.message)
return
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
# matplotlib
# concatenate x and y in order to sort
latest_msg = data_y[-1]
min_x = None
max_x = None
min_y = None
max_y = None
if isinstance(latest_msg, PlotData):
data = [latest_msg]
legend_size = 8
no_legend = False
elif isinstance(latest_msg, PlotDataArray):
data = latest_msg.data
legend_size = latest_msg.legend_font_size or 8
no_legend = latest_msg.no_legend
if latest_msg.min_x != latest_msg.max_x:
min_x = latest_msg.min_x
max_x = latest_msg.max_x
if latest_msg.min_y != latest_msg.max_y:
min_y = latest_msg.min_y
max_y = latest_msg.max_y
else:
rospy.logerr("Topic should be jsk_recognition_msgs/PlotData or jsk_recognition_msgs/PlotDataArray")
for d in data:
self.plot_one(d, axes)
xs = add_list([d.xs for d in data])
ys = add_list([d.ys for d in data])
if min_x is None:
min_x = min(xs)
if max_x is None:
max_x = max(xs)
if min_y is None:
min_y = min(ys)
if max_y is None:
max_y = max(ys)
axes.set_xlim(min_x, max_x)
axes.set_ylim(min_y, max_y)
# line fitting
if not no_legend and not self.no_legend:
axes.legend(prop={'size': legend_size})
axes.grid()
if self.xtitle:
axes.set_xlabel(self.xtitle)
if self.ytitle:
axes.set_ylabel(self.ytitle)
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class PlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, arguments=None, initial_topics=None):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._ordered_topics = []
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
# save command line arguments
self._arguments = arguments
self._initial_topics = initial_topics
def switch_data_plot_widget(self, data_plot):
self.enable_timer(enabled=False)
self.data_plot_layout.removeWidget(self.data_plot)
if self.data_plot is not None:
self.data_plot.close()
self.data_plot = data_plot
self.data_plot_layout.addWidget(self.data_plot)
# setup drag 'n drop
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
else:
for topic_name, rosdata in self._rosdata.items():
data_x, data_y = rosdata.next()
self.data_plot.add_curve(topic_name, topic_name, data_x,
data_y)
self._subscribed_topics_changed()
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(
event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
)
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
)
return
else:
topic_name = str(event.mimeData().text())
# check for numeric field type
is_numeric, is_array, message = is_slot_numeric(topic_name)
if is_numeric and not is_array:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
is_numeric, is_array, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_mass_button_clicked(self):
topics = str(self.mass_edit.toPlainText()).split()
for topic in topics:
self.add_topic(topic)
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
def update_plot(self):
if self.data_plot is not None:
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
self.data_plot.update_values(topic_name, data_x, data_y)
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' %
e)
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if self._arguments:
if self._arguments.start_paused:
self.pause_button.setChecked(True)
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer(self._rosdata)
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' %
topic_name)
return
self._ordered_topics.append(topic_name)
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self._ordered_topics.remove(topic_name)
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._ordered_topics = []
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class Plot2DWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName("Plot2DWidget")
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path("jsk_rqt_plugins"), "resource", "plot_histogram.ui")
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme("add"))
self.pause_button.setIcon(QIcon.fromTheme("media-playback-pause"))
self.clear_button.setIcon(QIcon.fromTheme("edit-clear"))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot("QDropEvent*")
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
# matplotlib
# concatenate x and y in order to sort
concatenated_data = zip(data_y[-1].xs, data_y[-1].ys)
if self.sort_x:
concatenated_data.sort(key=lambda x: x[0])
xs = [d[0] for d in concatenated_data]
ys = [d[1] for d in concatenated_data]
if self.is_line:
axes.plot(xs, ys, label=self.topic_with_field_name)
else:
axes.scatter(xs, ys)
# set limit
axes.set_xlim(min(xs), max(xs))
axes.set_ylim(min(ys), max(ys))
# line fitting
if self.fit_line:
X = np.array(data_y[-1].xs)
Y = np.array(data_y[-1].ys)
A = np.array([X, np.ones(len(X))])
A = A.T
a, b = np.linalg.lstsq(A, Y)[0]
axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b))
if self.fit_line_ransac:
model_ransac = linear_model.RANSACRegressor(
linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier
)
X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1))
Y = np.array(data_y[-1].ys)
model_ransac.fit(X, Y)
line_X = X
line_y_ransac = model_ransac.predict(line_X)
axes.plot(
line_X,
line_y_ransac,
"r--",
label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0]),
)
if not self.no_legend:
axes.legend(prop={"size": "8"})
axes.grid()
if self.xtitle:
axes.set_xlabel(self.xtitle)
if self.ytitle:
axes.set_ylabel(self.ytitle)
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Plot3DWidget(QWidget):
_redraw_interval = 40
def __init__(self,
initial_topics=None,
start_paused=False,
buffer_length=100,
use_poly=True,
no_legend=False):
super(Plot3DWidget, self).__init__()
self.setObjectName('Plot3DWidget')
self._buffer_length = buffer_length
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot3d.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatDataPlot3D(self, self._buffer_length, use_poly,
no_legend)
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or \
len(event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
) # NOQA
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
) # NOQA
return
else:
topic_name = str(event.mimeData().text())
# check for numeric field type
is_numeric, is_array, message = is_slot_numeric(topic_name)
if is_numeric and not is_array:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
is_numeric, is_array, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot(bool)
def on_autoscroll_checkbox_clicked(self, checked):
self.data_plot.autoscroll(checked)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
def update_plot(self):
if self.data_plot is not None:
needs_redraw = False
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
if data_x or data_y:
self.data_plot.update_values(topic_name, data_x,
data_y)
needs_redraw = True
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' %
e)
if needs_redraw:
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if not self.pause_button.isChecked():
# if pause button is not pressed,
# enable timer based on subscribed topics
self.enable_timer(self._rosdata)
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' %
topic_name) # NOQA
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
else:
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class Plot3DWidget(QWidget):
_redraw_interval = 40
def __init__(self, initial_topics=None, start_paused=False,
buffer_length=100, use_poly=True, no_legend=False):
super(Plot3DWidget, self).__init__()
self.setObjectName('Plot3DWidget')
self._buffer_length = buffer_length
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot3d.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatDataPlot3D(self, self._buffer_length,
use_poly, no_legend)
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
else:
topic_name = str(event.mimeData().text())
# check for numeric field type
is_numeric, is_array, message = is_slot_numeric(topic_name)
if is_numeric and not is_array:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
is_numeric, is_array, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot(bool)
def on_autoscroll_checkbox_clicked(self, checked):
self.data_plot.autoscroll(checked)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
def update_plot(self):
if self.data_plot is not None:
needs_redraw = False
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
if data_x or data_y:
self.data_plot.update_values(topic_name, data_x, data_y)
needs_redraw = True
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e)
if needs_redraw:
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer(self._rosdata)
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name)
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
else:
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class HistogramPlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
if self._rosdata.sub.data_class is HistogramWithRange:
xs = [y.count for y in data_y[-1].bins]
pos = [y.min_value for y in data_y[-1].bins]
widths = [y.max_value - y.min_value for y in data_y[-1].bins]
axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1])
else:
xs = data_y[-1]
pos = np.arange(len(xs))
widths = [1] * len(xs)
axes.set_xlim(xmin=0, xmax=len(xs))
#axes.xticks(range(5))
for p, x, w in zip(pos, xs, widths):
axes.bar(p, x, color='r', align='center', width=w)
axes.legend([self.topic_with_field_name], prop={'size': '8'})
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
