plot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from rqt_gui_py.plugin import Plugin
00004 from python_qt_binding import loadUi
00005 from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot
00006 from python_qt_binding.QtGui import QAction, QIcon, QMenu, QWidget
00007 from python_qt_binding.QtGui import QWidget, QVBoxLayout, QSizePolicy, QColor
00008 from rqt_py_common.topic_completer import TopicCompleter
00009 from matplotlib.colors import colorConverter
00010 from rqt_py_common.topic_helpers import is_slot_numeric
00011 from rqt_plot.rosplot import ROSData, RosPlotException
00012 from matplotlib.collections import PolyCollection, PathCollection, LineCollection
00013 import matplotlib
00014 
00015 import rospkg
00016 import rospy
00017 
00018 import os, sys
00019 import argparse
00020 
00021 try:
00022     from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas
00023 except ImportError:
00024     # work around bug in dateutil
00025     import sys
00026     import thread
00027     sys.modules['_thread'] = thread
00028     from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas
00029 from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg as NavigationToolbar
00030 from matplotlib.figure import Figure
00031 
00032 import numpy
00033 import matplotlib.pyplot as plt
00034 from mpl_toolkits.mplot3d import axes3d, Axes3D #<-- Note the capitalization! 
00035 
00036 class MatDataPlot3D(QWidget):
00037     class Canvas(FigureCanvas):
00038         """Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.)."""
00039         def __init__(self, parent=None):
00040             super(MatDataPlot3D.Canvas, self).__init__(Figure())
00041             #self.fig = fig = plt.figure()
00042             self.axes = self.figure.add_subplot(111, projection='3d')
00043             #self.axes = self.figure.gca(projection="3d")
00044             #self.axes.grid(True, color='gray')
00045             self.axes.set_xlabel('t')
00046             self.axes.set_xlim3d(0, 10)
00047             self.axes.set_ylabel('Y')
00048             self.axes.set_ylim3d(-1, 1)
00049             self.axes.set_zlabel('Z')
00050             self.axes.set_zlim3d(0, 1)
00051 
00052             self.figure.tight_layout()
00053             self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
00054             self.updateGeometry()
00055 
00056         def resizeEvent(self, event):
00057             super(MatDataPlot3D.Canvas, self).resizeEvent(event)
00058             self.figure.tight_layout()
00059 
00060     _colors = [QColor(c) for c in [Qt.red, Qt.blue, Qt.magenta, Qt.cyan, Qt.green, Qt.darkYellow, Qt.black, Qt.darkRed, Qt.gray, Qt.darkCyan]]
00061 
00062     def __init__(self, parent=None, buffer_length=100, use_poly=True, 
00063                  no_legend=False):
00064         super(MatDataPlot3D, self).__init__(parent)
00065         self._canvas = MatDataPlot3D.Canvas()
00066         self._use_poly = use_poly
00067         self._buffer_length = buffer_length
00068         self._toolbar = NavigationToolbar(self._canvas, self._canvas)
00069         vbox = QVBoxLayout()
00070         vbox.addWidget(self._toolbar)
00071         vbox.addWidget(self._canvas)
00072         self.setLayout(vbox)
00073         self._curves_verts = {}
00074         self._color_index = 0
00075         self._curves = {}
00076         self._no_legend = no_legend
00077         self._autoscroll = False
00078 
00079     def autoscroll(self, enabled=True):
00080         self._autoscroll = enabled
00081 
00082     def add_curve(self, curve_id, curve_name, x, y):
00083         color = QColor(self._colors[self._color_index % len(self._colors)])
00084         self._color_index += 1
00085         #line = self._canvas.axes.plot([], [], label=curve_name, linewidth=1, picker=5, color=color.name())[0]
00086         line = None
00087         self._curves[curve_id] = [[], [], line, [None, None], 
00088                                   (color.red() / 255.0, 
00089                                    color.green() / 255.0,
00090                                    color.blue() / 255.0,
00091                                    0.6)]
00092         self.update_values(curve_id, x, y)
00093         self._update_legend()
00094 
00095     def remove_curve(self, curve_id):
00096         curve_id = str(curve_id)
00097         if curve_id in self._curves:
00098             del self._curves[curve_id]
00099             del self._curves_verts[curve_id]
00100             self._update_legend()
00101 
00102     def _update_legend(self):
00103         if self._no_legend:
00104             return
00105         labels = self._curves.keys()
00106         handles = [plt.Rectangle((0, 0), 1, 1, fc=self._curves[labels[i]][4] ) for i in range(len(labels))]
00107         self._canvas.axes.legend(handles, labels, loc='upper left')
00108         
00109     @Slot(str, list, list)
00110     def update_values(self, curve_id, x, y):
00111         data_x, data_y, line, range_y,c = self._curves[curve_id]
00112         data_x.extend(x)
00113         data_y.extend(y)
00114         if len(data_x) > self._buffer_length:
00115             data_x = data_x[-self._buffer_length:]
00116             data_y = data_y[-self._buffer_length:]
00117             self._curves[curve_id][0] = data_x
00118             self._curves[curve_id][1] = data_y
00119         self._curves_verts[curve_id] = (data_x, data_y)
00120         if y:
00121             ymin = min(y)
00122             if range_y[0]:
00123                 ymin = min(ymin, range_y[0])
00124             range_y[0] = ymin
00125             ymax = max(y)
00126             if range_y[1]:
00127                 ymax = max(ymax, range_y[1])
00128             range_y[1] = ymax
00129     def redraw(self):
00130         self._canvas.axes.grid(True, color='gray')
00131         # Set axis bounds
00132         ymin = ymax = None
00133         xmax = 0
00134         xmin = sys.maxint
00135         for curve in self._curves.values():
00136             data_x, _, _, range_y, c = curve
00137             if len(data_x) == 0:
00138                 continue
00139             xmax = max(xmax, data_x[-1])
00140             xmin = min(xmin, data_x[0])
00141             if ymin is None:
00142                 ymin = range_y[0]
00143                 ymax = range_y[1]
00144             else:
00145                 ymin = min(range_y[0], ymin)
00146                 ymax = max(range_y[1], ymax)
00147 
00148             # pad the min/max
00149             # delta = max(ymax - ymin, 0.1)
00150             # ymin -= .05 * delta
00151             # ymax += .05 * delta
00152 
00153         if self._autoscroll and ymin is not None:
00154             self._canvas.axes.set_xbound(lower=xmin, upper=xmax)
00155             self._canvas.axes.set_zbound(lower=ymin, upper=ymax)
00156             self._canvas.axes.set_ybound(lower=0,
00157                                          upper=len(self._curves.keys()))
00158         # create poly object
00159         verts = []
00160         colors = []
00161         for curve_id in self._curves_verts.keys():
00162             (data_x, data_y) = self._curves_verts[curve_id]
00163             colors.append(self._curves[curve_id][4])
00164             if self._use_poly:
00165                 verts.append([(xmin, ymin)] + list(zip(data_x, data_y))
00166                              + [(xmax, ymin)])
00167             else:
00168                 verts.append(zip(data_x, data_y))
00169         line_num = len(self._curves.keys())
00170         if self._use_poly:
00171             poly = PolyCollection(verts, facecolors=colors, closed=False)
00172         else:
00173             poly = LineCollection(verts, colors=colors)
00174         poly.set_alpha(0.7)
00175         self._canvas.axes.cla()
00176         self._canvas.axes.add_collection3d(poly,
00177                                            zs=range(line_num), zdir='y')
00178         self._update_legend()
00179         self._canvas.draw()
00180 
00181 
00182 class Plot3D(Plugin):
00183     def __init__(self, context):
00184         super(Plot3D, self).__init__(context)
00185         self.setObjectName('Plot3D')
00186         self._args = self._parse_args(context.argv())
00187         self._widget = Plot3DWidget(initial_topics=self._args.topics, start_paused=self._args.start_paused, 
00188                                     buffer_length=self._args.buffer,
00189                                     use_poly=not self._args.show_line,
00190                                     no_legend=self._args.no_legend)
00191         context.add_widget(self._widget)
00192     def _parse_args(self, argv):
00193         parser = argparse.ArgumentParser(prog='rqt_3d_plot', add_help=False)
00194         Plot3D.add_arguments(parser)
00195         args = parser.parse_args(argv)
00196         topic_list = []
00197         for t in args.topics:
00198              # c_topics is the list of topics to plot
00199              c_topics = []
00200              # compute combined topic list, t == '/foo/bar1,/baz/bar2'
00201              for sub_t in [x for x in t.split(',') if x]:
00202                  # check for shorthand '/foo/field1:field2:field3'
00203                  if ':' in sub_t:
00204                      base = sub_t[:sub_t.find(':')]
00205                      # the first prefix includes a field name, so save then strip it off
00206                      c_topics.append(base)
00207                      if not '/' in base:
00208                          parser.error("%s must contain a topic and field name" % sub_t)
00209                      base = base[:base.rfind('/')]
00210 
00211                      # compute the rest of the field names
00212                      fields = sub_t.split(':')[1:]
00213                      c_topics.extend(["%s/%s" % (base, f) for f in fields if f])
00214                  else:
00215                      c_topics.append(sub_t)
00216              # #1053: resolve command-line topic names
00217              import rosgraph
00218              c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n) for n in c_topics]
00219              if type(c_topics) == list:
00220                  topic_list.extend(c_topics)
00221              else:
00222                  topic_list.append(c_topics)
00223         args.topics = topic_list
00224 
00225         return args
00226     @staticmethod
00227     def add_arguments(parser):
00228         group = parser.add_argument_group('Options for rqt_plot plugin')
00229         group.add_argument('-P', '--pause', action='store_true', dest='start_paused',
00230             help='Start in paused state')
00231         group.add_argument('-L', '--line', action='store_true', dest='show_line',
00232             help='Show lines rather than polygon representation')
00233         group.add_argument('--no-legend', action='store_true', dest='no_legend',
00234             help='do not show legend')
00235         group.add_argument('-B', '--buffer', dest='buffer', action="store",
00236             help='the length of the buffer', default=100, type=int)
00237         # group.add_argument('-e', '--empty', action='store_true', dest='start_empty',
00238         #     help='Start without restoring previous topics')
00239         group.add_argument('topics', nargs='*', default=[], help='Topics to plot')
00240 
00241 class Plot3DWidget(QWidget):
00242     _redraw_interval = 40
00243 
00244     def __init__(self, initial_topics=None, start_paused=False, 
00245                  buffer_length=100, use_poly=True, no_legend=False):
00246         super(Plot3DWidget, self).__init__()
00247         self.setObjectName('Plot3DWidget')
00248         self._buffer_length = buffer_length
00249         self._initial_topics = initial_topics
00250 
00251         rp = rospkg.RosPack()
00252         ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 
00253                                'resource', 'plot3d.ui')
00254         loadUi(ui_file, self)
00255         self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
00256         self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
00257         self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
00258         self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
00259         self.data_plot = MatDataPlot3D(self, self._buffer_length, 
00260                                        use_poly, no_legend)
00261         self.data_plot_layout.addWidget(self.data_plot)
00262         self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
00263         self.data_plot.dropEvent = self.dropEvent
00264         self.data_plot.dragEnterEvent = self.dragEnterEvent
00265 
00266         self.subscribe_topic_button.setEnabled(False)
00267         if start_paused:
00268             self.pause_button.setChecked(True)
00269 
00270         self._topic_completer = TopicCompleter(self.topic_edit)
00271         self._topic_completer.update_topics()
00272         self.topic_edit.setCompleter(self._topic_completer)
00273 
00274         self._start_time = rospy.get_time()
00275         self._rosdata = {}
00276         self._remove_topic_menu = QMenu()
00277 
00278         # init and start update timer for plot
00279         self._update_plot_timer = QTimer(self)
00280         self._update_plot_timer.timeout.connect(self.update_plot)
00281         if self._initial_topics:
00282             for topic_name in self._initial_topics:
00283                 self.add_topic(topic_name)
00284             self._initial_topics = None
00285 
00286     @Slot('QDragEnterEvent*')
00287     def dragEnterEvent(self, event):
00288         # get topic name
00289         if not event.mimeData().hasText():
00290             if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
00291                 qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
00292                 return
00293             item = event.source().selectedItems()[0]
00294             topic_name = item.data(0, Qt.UserRole)
00295             if topic_name == None:
00296                 qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
00297                 return
00298         else:
00299             topic_name = str(event.mimeData().text())
00300 
00301         # check for numeric field type
00302         is_numeric, is_array, message = is_slot_numeric(topic_name)
00303         if is_numeric and not is_array:
00304             event.acceptProposedAction()
00305         else:
00306             qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
00307 
00308     @Slot('QDropEvent*')
00309     def dropEvent(self, event):
00310         if event.mimeData().hasText():
00311             topic_name = str(event.mimeData().text())
00312         else:
00313             droped_item = event.source().selectedItems()[0]
00314             topic_name = str(droped_item.data(0, Qt.UserRole))
00315         self.add_topic(topic_name)
00316 
00317     @Slot(str)
00318     def on_topic_edit_textChanged(self, topic_name):
00319         # on empty topic name, update topics
00320         if topic_name in ('', '/'):
00321             self._topic_completer.update_topics()
00322 
00323         is_numeric, is_array, message = is_slot_numeric(topic_name)
00324         self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
00325         self.subscribe_topic_button.setToolTip(message)
00326 
00327     @Slot()
00328     def on_topic_edit_returnPressed(self):
00329         if self.subscribe_topic_button.isEnabled():
00330             self.add_topic(str(self.topic_edit.text()))
00331 
00332     @Slot()
00333     def on_subscribe_topic_button_clicked(self):
00334         self.add_topic(str(self.topic_edit.text()))
00335 
00336     @Slot(bool)
00337     def on_pause_button_clicked(self, checked):
00338         self.enable_timer(not checked)
00339 
00340     @Slot(bool)
00341     def on_autoscroll_checkbox_clicked(self, checked):
00342         self.data_plot.autoscroll(checked)
00343 
00344     @Slot()
00345     def on_clear_button_clicked(self):
00346         self.clean_up_subscribers()
00347 
00348     def update_plot(self):
00349         if self.data_plot is not None:
00350             needs_redraw = False
00351             for topic_name, rosdata in self._rosdata.items():
00352                 try:
00353                     data_x, data_y = rosdata.next()
00354                     if data_x or data_y:
00355                         self.data_plot.update_values(topic_name, data_x, data_y)
00356                         needs_redraw = True
00357                 except RosPlotException as e:
00358                     qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e)
00359             if needs_redraw:
00360                 self.data_plot.redraw()
00361 
00362     def _subscribed_topics_changed(self):
00363         self._update_remove_topic_menu()
00364         if not self.pause_button.isChecked():
00365             # if pause button is not pressed, enable timer based on subscribed topics
00366             self.enable_timer(self._rosdata)
00367 
00368     def _update_remove_topic_menu(self):
00369         def make_remove_topic_function(x):
00370             return lambda: self.remove_topic(x)
00371 
00372         self._remove_topic_menu.clear()
00373         for topic_name in sorted(self._rosdata.keys()):
00374             action = QAction(topic_name, self._remove_topic_menu)
00375             action.triggered.connect(make_remove_topic_function(topic_name))
00376             self._remove_topic_menu.addAction(action)
00377 
00378         self.remove_topic_button.setMenu(self._remove_topic_menu)
00379 
00380     def add_topic(self, topic_name):
00381         if topic_name in self._rosdata:
00382             qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name)
00383             return
00384 
00385         self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
00386         if self._rosdata[topic_name].error is not None:
00387             qWarning(str(self._rosdata[topic_name].error))
00388             del self._rosdata[topic_name]
00389         else:
00390             data_x, data_y = self._rosdata[topic_name].next()
00391             self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
00392 
00393             self._subscribed_topics_changed()
00394 
00395     def remove_topic(self, topic_name):
00396         self._rosdata[topic_name].close()
00397         del self._rosdata[topic_name]
00398         self.data_plot.remove_curve(topic_name)
00399 
00400         self._subscribed_topics_changed()
00401 
00402     def clean_up_subscribers(self):
00403         for topic_name, rosdata in self._rosdata.items():
00404             rosdata.close()
00405             self.data_plot.remove_curve(topic_name)
00406         self._rosdata = {}
00407 
00408         self._subscribed_topics_changed()
00409 
00410     def enable_timer(self, enabled=True):
00411         if enabled:
00412             self._update_plot_timer.start(self._redraw_interval)
00413         else:
00414             self._update_plot_timer.stop()


jsk_rqt_plugins
Author(s):
autogenerated on Sun Sep 13 2015 22:29:48