plot.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import argparse
4 from distutils.version import LooseVersion
5 import os
6 import sys
7 
8 import matplotlib
9 from matplotlib.collections import LineCollection
10 from matplotlib.collections import PathCollection
11 from matplotlib.collections import PolyCollection
12 from matplotlib.colors import colorConverter
13 from matplotlib.figure import Figure
14 import matplotlib.pyplot as plt
15 from mpl_toolkits.mplot3d import axes3d
16 from mpl_toolkits.mplot3d import Axes3D # <-- Note the capitalization!
17 import numpy
18 import python_qt_binding
19 from python_qt_binding import loadUi
20 from python_qt_binding.QtCore import Qt
21 from python_qt_binding.QtCore import QTimer
22 from python_qt_binding.QtCore import qWarning
23 from python_qt_binding.QtCore import Slot
24 from python_qt_binding.QtGui import QColor
25 from python_qt_binding.QtGui import QIcon
26 
27 import rospkg
28 import rospy
29 from rqt_gui_py.plugin import Plugin
30 from rqt_plot.rosplot import ROSData, RosPlotException
31 from rqt_py_common.topic_completer import TopicCompleter
32 from rqt_py_common.topic_helpers import is_slot_numeric
33 
34 # Support both qt4 and qt5
35 if LooseVersion(python_qt_binding.QT_BINDING_VERSION).version[0] >= 5:
36  from python_qt_binding.QtWidgets import QAction
37  from python_qt_binding.QtWidgets import QMenu
38  from python_qt_binding.QtWidgets import QSizePolicy
39  from python_qt_binding.QtWidgets import QVBoxLayout
40  from python_qt_binding.QtWidgets import QWidget
41  try:
42  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
43  as FigureCanvas
44  except ImportError:
45  # work around bug in dateutil
46  import thread
47  sys.modules['_thread'] = thread
48  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
49  as FigureCanvas
50  try:
51  from matplotlib.backends.backend_qt5agg \
52  import NavigationToolbar2QTAgg as NavigationToolbar
53  except ImportError:
54  from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT \
55  as NavigationToolbar
56 
57 else:
58  from python_qt_binding.QtGui import QAction
59  from python_qt_binding.QtGui import QMenu
60  from python_qt_binding.QtGui import QSizePolicy
61  from python_qt_binding.QtGui import QVBoxLayout
62  from python_qt_binding.QtGui import QWidget
63  try:
64  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
65  as FigureCanvas
66  except ImportError:
67  # work around bug in dateutil
68  import thread
69  sys.modules['_thread'] = thread
70  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
71  as FigureCanvas
72  try:
73  from matplotlib.backends.backend_qt4agg \
74  import NavigationToolbar2QTAgg as NavigationToolbar
75  except ImportError:
76  from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT \
77  as NavigationToolbar
78 
79 
80 class MatDataPlot3D(QWidget):
81  class Canvas(FigureCanvas):
82  """Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.).
83 """
84  def __init__(self, parent=None):
85  super(MatDataPlot3D.Canvas, self).__init__(Figure())
86  # self.fig = fig = plt.figure()
87  self.axes = self.figure.add_subplot(111, projection='3d')
88  # self.axes = self.figure.gca(projection="3d")
89  # self.axes.grid(True, color='gray')
90  self.axes.set_xlabel('t')
91  self.axes.set_xlim3d(0, 10)
92  self.axes.set_ylabel('Y')
93  self.axes.set_ylim3d(-1, 1)
94  self.axes.set_zlabel('Z')
95  self.axes.set_zlim3d(0, 1)
96 
97  self.figure.tight_layout()
98  self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
99  self.updateGeometry()
100 
101  def resizeEvent(self, event):
102  super(MatDataPlot3D.Canvas, self).resizeEvent(event)
103  self.figure.tight_layout()
104 
105  _colors = [QColor(c) for c in [
106  Qt.red, Qt.blue, Qt.magenta, Qt.cyan, Qt.green, Qt.darkYellow,
107  Qt.black, Qt.darkRed, Qt.gray, Qt.darkCyan]]
108 
109  def __init__(self, parent=None, buffer_length=100, use_poly=True,
110  no_legend=False):
111  super(MatDataPlot3D, self).__init__(parent)
113  self._use_poly = use_poly
114  self._buffer_length = buffer_length
115  self._toolbar = NavigationToolbar(self._canvas, self._canvas)
116  vbox = QVBoxLayout()
117  vbox.addWidget(self._toolbar)
118  vbox.addWidget(self._canvas)
119  self.setLayout(vbox)
120  self._curves_verts = {}
121  self._color_index = 0
122  self._curves = {}
123  self._no_legend = no_legend
124  self._autoscroll = False
125 
126  def autoscroll(self, enabled=True):
127  self._autoscroll = enabled
128 
129  def add_curve(self, curve_id, curve_name, x, y):
130  color = QColor(self._colors[self._color_index % len(self._colors)])
131  self._color_index += 1
132  # line = self._canvas.axes.plot(
133  # [], [], label=curve_name, linewidth=1, picker=5,
134  # color=color.name())[0]
135  line = None
136  self._curves[curve_id] = [[], [], line, [None, None],
137  (color.red() / 255.0,
138  color.green() / 255.0,
139  color.blue() / 255.0,
140  0.6)]
141  self.update_values(curve_id, x, y)
142  self._update_legend()
143 
144  def remove_curve(self, curve_id):
145  curve_id = str(curve_id)
146  if curve_id in self._curves:
147  del self._curves[curve_id]
148  del self._curves_verts[curve_id]
149  self._update_legend()
150 
151  def _update_legend(self):
152  if self._no_legend:
153  return
154  labels = self._curves.keys()
155  handles = [
156  plt.Rectangle((0, 0), 1, 1, fc=self._curves[labels[i]][4])
157  for i in range(len(labels))]
158  self._canvas.axes.legend(handles, labels, loc='upper left')
159 
160  @Slot(str, list, list)
161  def update_values(self, curve_id, x, y):
162  data_x, data_y, line, range_y, c = self._curves[curve_id]
163  data_x.extend(x)
164  data_y.extend(y)
165  if len(data_x) > self._buffer_length:
166  data_x = data_x[-self._buffer_length:]
167  data_y = data_y[-self._buffer_length:]
168  self._curves[curve_id][0] = data_x
169  self._curves[curve_id][1] = data_y
170  self._curves_verts[curve_id] = (data_x, data_y)
171  if y:
172  ymin = min(y)
173  if range_y[0]:
174  ymin = min(ymin, range_y[0])
175  range_y[0] = ymin
176  ymax = max(y)
177  if range_y[1]:
178  ymax = max(ymax, range_y[1])
179  range_y[1] = ymax
180 
181  def redraw(self):
182  self._canvas.axes.grid(True, color='gray')
183  # Set axis bounds
184  ymin = ymax = None
185  xmax = 0
186  xmin = sys.maxint
187  for curve in self._curves.values():
188  data_x, _, _, range_y, c = curve
189  if len(data_x) == 0:
190  continue
191  xmax = max(xmax, data_x[-1])
192  xmin = min(xmin, data_x[0])
193  if ymin is None:
194  ymin = range_y[0]
195  ymax = range_y[1]
196  else:
197  ymin = min(range_y[0], ymin)
198  ymax = max(range_y[1], ymax)
199 
200  # pad the min/max
201  # delta = max(ymax - ymin, 0.1)
202  # ymin -= .05 * delta
203  # ymax += .05 * delta
204 
205  if self._autoscroll and ymin is not None:
206  self._canvas.axes.set_xbound(lower=xmin, upper=xmax)
207  self._canvas.axes.set_zbound(lower=ymin, upper=ymax)
208  self._canvas.axes.set_ybound(lower=0,
209  upper=len(self._curves.keys()))
210  # create poly object
211  verts = []
212  colors = []
213  for curve_id in self._curves_verts.keys():
214  (data_x, data_y) = self._curves_verts[curve_id]
215  colors.append(self._curves[curve_id][4])
216  if self._use_poly:
217  verts.append([(xmin, ymin)] + list(zip(data_x, data_y))
218  + [(xmax, ymin)])
219  else:
220  verts.append(zip(data_x, data_y))
221  line_num = len(self._curves.keys())
222  if self._use_poly:
223  poly = PolyCollection(verts, facecolors=colors, closed=False)
224  else:
225  poly = LineCollection(verts, colors=colors)
226  poly.set_alpha(0.7)
227  self._canvas.axes.cla()
228  self._canvas.axes.add_collection3d(poly,
229  zs=range(line_num), zdir='y')
230  self._update_legend()
231  self._canvas.draw()
232 
233 
234 class Plot3D(Plugin):
235  def __init__(self, context):
236  super(Plot3D, self).__init__(context)
237  self.setObjectName('Plot3D')
238  self._args = self._parse_args(context.argv())
240  initial_topics=self._args.topics,
241  start_paused=self._args.start_paused,
242  buffer_length=self._args.buffer,
243  use_poly=not self._args.show_line,
244  no_legend=self._args.no_legend)
245  context.add_widget(self._widget)
246 
247  def _parse_args(self, argv):
248  parser = argparse.ArgumentParser(prog='rqt_3d_plot', add_help=False)
249  Plot3D.add_arguments(parser)
250  args = parser.parse_args(argv)
251  topic_list = []
252  for t in args.topics:
253  # c_topics is the list of topics to plot
254  c_topics = []
255  # compute combined topic list, t == '/foo/bar1,/baz/bar2'
256  for sub_t in [x for x in t.split(',') if x]:
257  # check for shorthand '/foo/field1:field2:field3'
258  if ':' in sub_t:
259  base = sub_t[:sub_t.find(':')]
260  # the first prefix includes a field name,
261  # so save then strip it off
262  c_topics.append(base)
263  if not '/' in base:
264  parser.error(
265  "%s must contain a topic and field name" % sub_t)
266  base = base[:base.rfind('/')]
267 
268  # compute the rest of the field names
269  fields = sub_t.split(':')[1:]
270  c_topics.extend(["%s/%s" % (base, f) for f in fields if f])
271  else:
272  c_topics.append(sub_t)
273  # #1053: resolve command-line topic names
274  import rosgraph
275  c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n)
276  for n in c_topics]
277  if type(c_topics) == list:
278  topic_list.extend(c_topics)
279  else:
280  topic_list.append(c_topics)
281  args.topics = topic_list
282 
283  return args
284 
285  @staticmethod
286  def add_arguments(parser):
287  group = parser.add_argument_group('Options for rqt_plot plugin')
288  group.add_argument(
289  '-P', '--pause', action='store_true', dest='start_paused',
290  help='Start in paused state')
291  group.add_argument(
292  '-L', '--line', action='store_true', dest='show_line',
293  help='Show lines rather than polygon representation')
294  group.add_argument(
295  '--no-legend', action='store_true', dest='no_legend',
296  help='do not show legend')
297  group.add_argument(
298  '-B', '--buffer', dest='buffer', action="store",
299  help='the length of the buffer', default=100, type=int)
300  # group.add_argument(
301  # '-e', '--empty', action='store_true', dest='start_empty',
302  # help='Start without restoring previous topics')
303  group.add_argument(
304  'topics', nargs='*', default=[], help='Topics to plot')
305 
306 
307 class Plot3DWidget(QWidget):
308  _redraw_interval = 40
309 
310  def __init__(self, initial_topics=None, start_paused=False,
311  buffer_length=100, use_poly=True, no_legend=False):
312  super(Plot3DWidget, self).__init__()
313  self.setObjectName('Plot3DWidget')
314  self._buffer_length = buffer_length
315  self._initial_topics = initial_topics
316 
317  rp = rospkg.RosPack()
318  ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
319  'resource', 'plot3d.ui')
320  loadUi(ui_file, self)
321  self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
322  self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
323  self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
324  self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
326  use_poly, no_legend)
327  self.data_plot_layout.addWidget(self.data_plot)
328  self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
329  self.data_plot.dropEvent = self.dropEvent
330  self.data_plot.dragEnterEvent = self.dragEnterEvent
331 
332  self.subscribe_topic_button.setEnabled(False)
333  if start_paused:
334  self.pause_button.setChecked(True)
335 
336  self._topic_completer = TopicCompleter(self.topic_edit)
337  self._topic_completer.update_topics()
338  self.topic_edit.setCompleter(self._topic_completer)
339 
340  self._start_time = rospy.get_time()
341  self._rosdata = {}
342  self._remove_topic_menu = QMenu()
343 
344  # init and start update timer for plot
345  self._update_plot_timer = QTimer(self)
346  self._update_plot_timer.timeout.connect(self.update_plot)
347  if self._initial_topics:
348  for topic_name in self._initial_topics:
349  self.add_topic(topic_name)
350  self._initial_topics = None
351 
352  @Slot('QDragEnterEvent*')
353  def dragEnterEvent(self, event):
354  # get topic name
355  if not event.mimeData().hasText():
356  if not hasattr(event.source(), 'selectedItems') or \
357  len(event.source().selectedItems()) == 0:
358  qWarning(
359  'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0') # NOQA
360  return
361  item = event.source().selectedItems()[0]
362  topic_name = item.data(0, Qt.UserRole)
363  if topic_name == None:
364  qWarning(
365  'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)') # NOQA
366  return
367  else:
368  topic_name = str(event.mimeData().text())
369 
370  # check for numeric field type
371  is_numeric, is_array, message = is_slot_numeric(topic_name)
372  if is_numeric and not is_array:
373  event.acceptProposedAction()
374  else:
375  qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
376 
377  @Slot('QDropEvent*')
378  def dropEvent(self, event):
379  if event.mimeData().hasText():
380  topic_name = str(event.mimeData().text())
381  else:
382  droped_item = event.source().selectedItems()[0]
383  topic_name = str(droped_item.data(0, Qt.UserRole))
384  self.add_topic(topic_name)
385 
386  @Slot(str)
387  def on_topic_edit_textChanged(self, topic_name):
388  # on empty topic name, update topics
389  if topic_name in ('', '/'):
390  self._topic_completer.update_topics()
391 
392  is_numeric, is_array, message = is_slot_numeric(topic_name)
393  self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
394  self.subscribe_topic_button.setToolTip(message)
395 
396  @Slot()
398  if self.subscribe_topic_button.isEnabled():
399  self.add_topic(str(self.topic_edit.text()))
400 
401  @Slot()
403  self.add_topic(str(self.topic_edit.text()))
404 
405  @Slot(bool)
406  def on_pause_button_clicked(self, checked):
407  self.enable_timer(not checked)
408 
409  @Slot(bool)
410  def on_autoscroll_checkbox_clicked(self, checked):
411  self.data_plot.autoscroll(checked)
412 
413  @Slot()
415  self.clean_up_subscribers()
416 
417  def update_plot(self):
418  if self.data_plot is not None:
419  needs_redraw = False
420  for topic_name, rosdata in self._rosdata.items():
421  try:
422  data_x, data_y = rosdata.next()
423  if data_x or data_y:
424  self.data_plot.update_values(
425  topic_name, data_x, data_y)
426  needs_redraw = True
427  except RosPlotException as e:
428  qWarning(
429  'PlotWidget.update_plot(): error in rosplot: %s' % e)
430  if needs_redraw:
431  self.data_plot.redraw()
432 
435  if not self.pause_button.isChecked():
436  # if pause button is not pressed,
437  # enable timer based on subscribed topics
438  self.enable_timer(self._rosdata)
439 
441  def make_remove_topic_function(x):
442  return lambda: self.remove_topic(x)
443 
444  self._remove_topic_menu.clear()
445  for topic_name in sorted(self._rosdata.keys()):
446  action = QAction(topic_name, self._remove_topic_menu)
447  action.triggered.connect(make_remove_topic_function(topic_name))
448  self._remove_topic_menu.addAction(action)
449 
450  self.remove_topic_button.setMenu(self._remove_topic_menu)
451 
452  def add_topic(self, topic_name):
453  if topic_name in self._rosdata:
454  qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) # NOQA
455  return
456 
457  self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
458  if self._rosdata[topic_name].error is not None:
459  qWarning(str(self._rosdata[topic_name].error))
460  del self._rosdata[topic_name]
461  else:
462  data_x, data_y = self._rosdata[topic_name].next()
463  self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
464 
466 
467  def remove_topic(self, topic_name):
468  self._rosdata[topic_name].close()
469  del self._rosdata[topic_name]
470  self.data_plot.remove_curve(topic_name)
471 
473 
475  for topic_name, rosdata in self._rosdata.items():
476  rosdata.close()
477  self.data_plot.remove_curve(topic_name)
478  self._rosdata = {}
479 
481 
482  def enable_timer(self, enabled=True):
483  if enabled:
484  self._update_plot_timer.start(self._redraw_interval)
485  else:
486  self._update_plot_timer.stop()
def enable_timer(self, enabled=True)
Definition: plot.py:482
def on_topic_edit_returnPressed(self)
Definition: plot.py:397
def update_values(self, curve_id, x, y)
Definition: plot.py:161
def __init__(self, parent=None, buffer_length=100, use_poly=True, no_legend=False)
Definition: plot.py:110
def remove_curve(self, curve_id)
Definition: plot.py:144
def dropEvent(self, event)
Definition: plot.py:378
def autoscroll(self, enabled=True)
Definition: plot.py:126
def remove_topic(self, topic_name)
Definition: plot.py:467
def on_subscribe_topic_button_clicked(self)
Definition: plot.py:402
def on_topic_edit_textChanged(self, topic_name)
Definition: plot.py:387
def _subscribed_topics_changed(self)
Definition: plot.py:433
def on_pause_button_clicked(self, checked)
Definition: plot.py:406
def __init__(self, initial_topics=None, start_paused=False, buffer_length=100, use_poly=True, no_legend=False)
Definition: plot.py:311
def __init__(self, parent=None)
Definition: plot.py:84
def __init__(self, context)
Definition: plot.py:235
def add_topic(self, topic_name)
Definition: plot.py:452
def add_arguments(parser)
Definition: plot.py:286
def _parse_args(self, argv)
Definition: plot.py:247
def add_curve(self, curve_id, curve_name, x, y)
Definition: plot.py:129
def dragEnterEvent(self, event)
Definition: plot.py:353
def _update_remove_topic_menu(self)
Definition: plot.py:440
def on_autoscroll_checkbox_clicked(self, checked)
Definition: plot.py:410


jsk_rqt_plugins
Author(s):
autogenerated on Fri Feb 5 2021 04:03:20