plot_2d.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import argparse
4 import os
5 import sys
6 
7 from cStringIO import StringIO
8 import cv2
9 from cv_bridge import CvBridge
10 from distutils.version import LooseVersion
11 from matplotlib.figure import Figure
12 import numpy as np
13 import python_qt_binding
14 from python_qt_binding import loadUi
15 from python_qt_binding.QtCore import Qt
16 from python_qt_binding.QtCore import QTimer
17 from python_qt_binding.QtCore import Slot
18 from python_qt_binding.QtGui import QIcon
19 import rospkg
20 import rospy
21 from rqt_gui_py.plugin import Plugin
22 from rqt_plot.rosplot import ROSData as _ROSData
23 from rqt_plot.rosplot import RosPlotException
24 from rqt_py_common.topic_completer import TopicCompleter
25 from sensor_msgs.msg import Image
26 from sklearn import linear_model
27 
28 from jsk_recognition_msgs.msg import PlotData
29 from jsk_recognition_msgs.msg import PlotDataArray
30 
31 # qt5 in kinetic
32 if LooseVersion(python_qt_binding.QT_BINDING_VERSION).version[0] >= 5:
33  from python_qt_binding.QtWidgets import QSizePolicy
34  from python_qt_binding.QtWidgets import QVBoxLayout
35  from python_qt_binding.QtWidgets import QWidget
36  try:
37  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
38  as FigureCanvas
39  except ImportError:
40  # work around bug in dateutil
41  import thread
42  sys.modules['_thread'] = thread
43  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
44  as FigureCanvas
45  try:
46  from matplotlib.backends.backend_qt5agg import NavigationToolbar2QTAgg \
47  as NavigationToolbar
48  except ImportError:
49  from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT \
50  as NavigationToolbar
51 else:
52  from python_qt_binding.QtGui import QSizePolicy
53  from python_qt_binding.QtGui import QVBoxLayout
54  from python_qt_binding.QtGui import QWidget
55  try:
56  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
57  as FigureCanvas
58  except ImportError:
59  # work around bug in dateutil
60  import thread
61  sys.modules['_thread'] = thread
62  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
63  as FigureCanvas
64  try:
65  from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg \
66  as NavigationToolbar
67  except ImportError:
68  from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT \
69  as NavigationToolbar
70 
71 
72 
73 class ROSData(_ROSData):
74  def _get_data(self, msg):
75  val = msg
76  try:
77  if not self.field_evals:
78  return val
79  for f in self.field_evals:
80  val = f(val)
81  return val
82  except IndexError:
84  "{0} index error for: {1}".format(
85  self.name, str(val).replace('\n', ', ')))
86  except TypeError:
87  self.error = RosPlotException(
88  "{0} value was not numeric: {1}".format(
89  self.name, val))
90 
91 
92 def add_list(xss):
93 
94  "xss = [[0, 1, 2, ...], [0, 1, 2, ...], ...]"
95 
96  ret = []
97  for xs in xss:
98  ret.extend(xs)
99  return ret
100 
101 
102 class Plot2D(Plugin):
103  def __init__(self, context):
104  super(Plot2D, self).__init__(context)
105  self.setObjectName('Plot2D')
106  self._args = self._parse_args(context.argv())
107  self._widget = Plot2DWidget(self._args.topics)
108  self._widget.is_line = self._args.line
109  self._widget.fit_line = self._args.fit_line
110  self._widget.fit_line_ransac = self._args.fit_line_ransac
111  outlier = self._args.fit_line_ransac_outlier
112  self._widget.fit_line_ransac_outlier = outlier
113  self._widget.xtitle = self._args.xtitle
114  self._widget.ytitle = self._args.ytitle
115  self._widget.no_legend = self._args.no_legend
116  self._widget.sort_x = self._args.sort_x
117  context.add_widget(self._widget)
118 
119  def _parse_args(self, argv):
120  parser = argparse.ArgumentParser(
121  prog='rqt_histogram_plot', add_help=False)
122  Plot2D.add_arguments(parser)
123  args = parser.parse_args(argv)
124  return args
125 
126  @staticmethod
127  def add_arguments(parser):
128  group = parser.add_argument_group(
129  'Options for rqt_histogram plugin')
130  group.add_argument(
131  'topics', nargs='?', default=[], help='Topics to plot')
132  group.add_argument(
133  '--line', action="store_true",
134  help="Plot with lines instead of scatter")
135  group.add_argument(
136  '--fit-line', action="store_true",
137  help="Plot line with least-square fitting")
138  group.add_argument(
139  '--fit-line-ransac', action="store_true",
140  help="Plot line with RANSAC")
141  group.add_argument(
142  '--fit-line-ransac-outlier', type=float, default=0.1,
143  help="Plot line with RANSAC")
144  group.add_argument('--xtitle', help="Title in X axis")
145  group.add_argument('--ytitle', help="Title in Y axis")
146  group.add_argument('--no-legend', action="store_true")
147  group.add_argument('--sort-x', action="store_true")
148 
149 
150 class Plot2DWidget(QWidget):
151  _redraw_interval = 10
152 
153  def __init__(self, topics):
154  super(Plot2DWidget, self).__init__()
155  self.setObjectName('Plot2DWidget')
156  rp = rospkg.RosPack()
157  ui_file = os.path.join(
158  rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui')
159  loadUi(ui_file, self)
160  self.cv_bridge = CvBridge()
161  self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
162  self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
163  self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
164  self.data_plot = MatPlot2D(self)
165  self.data_plot_layout.addWidget(self.data_plot)
166  self._topic_completer = TopicCompleter(self.topic_edit)
167  self._topic_completer.update_topics()
168  self.topic_edit.setCompleter(self._topic_completer)
169  self.data_plot.dropEvent = self.dropEvent
170  self.data_plot.dragEnterEvent = self.dragEnterEvent
171  self._start_time = rospy.get_time()
172  self._rosdata = None
173  if len(topics) != 0:
174  self.subscribe_topic(topics)
175  self._update_plot_timer = QTimer(self)
176  self._update_plot_timer.timeout.connect(self.update_plot)
177  self._update_plot_timer.start(self._redraw_interval)
178 
179  @Slot('QDropEvent*')
180  def dropEvent(self, event):
181  if event.mimeData().hasText():
182  topic_name = str(event.mimeData().text())
183  else:
184  droped_item = event.source().selectedItems()[0]
185  topic_name = str(droped_item.data(0, Qt.UserRole))
186  self.subscribe_topic(topic_name)
187 
188  @Slot()
190  "callback function when form is entered"
191  if self.subscribe_topic_button.isEnabled():
192  self.subscribe_topic(str(self.topic_edit.text()))
193 
194  @Slot()
196  self.subscribe_topic(str(self.topic_edit.text()))
197 
198  def subscribe_topic(self, topic_name):
199  rospy.loginfo("subscribe topic")
200  self.topic_with_field_name = topic_name
201  self.pub_image = rospy.Publisher(
202  topic_name + "/plot_image", Image, queue_size=1)
203  if not self._rosdata:
204  self._rosdata = ROSData(topic_name, self._start_time)
205  else:
206  if self._rosdata != topic_name:
207  self._rosdata.close()
208  self.data_plot.clear()
209  self._rosdata = ROSData(topic_name, self._start_time)
210  else:
211  rospy.logwarn("%s is already subscribed", topic_name)
212 
213  def enable_timer(self, enabled=True):
214  if enabled:
215  self._update_plot_timer.start(self._redraw_interval)
216  else:
217  self._update_plot_timer.stop()
218 
219  @Slot()
221  self.data_plot.clear()
222 
223  @Slot(bool)
224  def on_pause_button_clicked(self, checked):
225  self.enable_timer(not checked)
226 
227  def plot_one(self, msg, axes):
228  concatenated_data = zip(msg.xs, msg.ys)
229  if self.sort_x:
230  concatenated_data.sort(key=lambda x: x[0])
231  xs = [d[0] for d in concatenated_data]
232  ys = [d[1] for d in concatenated_data]
233  if self.is_line or msg.type is PlotData.LINE:
234  axes.plot(xs, ys,
235  label=msg.label or self.topic_with_field_name)
236  else:
237  axes.scatter(xs, ys,
238  label=msg.label or self.topic_with_field_name)
239  if msg.fit_line or self.fit_line:
240  X = np.array(msg.xs)
241  Y = np.array(msg.ys)
242  A = np.array([X, np.ones(len(X))])
243  A = A.T
244  a, b = np.linalg.lstsq(A, Y, rcond=-1)[0]
245  axes.plot(X, (a*X+b), "g--", label="{0} x + {1}".format(a, b))
246  if msg.fit_line_ransac or self.fit_line_ransac:
247  model_ransac = linear_model.RANSACRegressor(
248  linear_model.LinearRegression(), min_samples=2,
249  residual_threshold=self.fit_line_ransac_outlier)
250  X = np.array(msg.xs).reshape((len(msg.xs), 1))
251  Y = np.array(msg.ys)
252  model_ransac.fit(X, Y)
253  line_X = X
254  line_y_ransac = model_ransac.predict(line_X)
255  if len(model_ransac.estimator_.coef_) == 1:
256  coef = model_ransac.estimator_.coef_[0]
257  else:
258  coef = model_ransac.estimator_.coef_[0][0]
259  if not isinstance(model_ransac.estimator_.intercept_, list):
260  intercept = model_ransac.estimator_.intercept_
261  else:
262  intercept = model_ransac.estimator_.intercept_[0]
263  axes.plot(
264  line_X, line_y_ransac, "r--",
265  label="{0} x + {1}".format(coef, intercept))
266 
267  def update_plot(self):
268  if not self._rosdata:
269  return
270  try:
271  data_x, data_y = self._rosdata.next()
272  except RosPlotException as e:
273  rospy.logerr("Exception in subscribing topic")
274  rospy.logerr(e.message)
275  return
276  if len(data_y) == 0:
277  return
278  axes = self.data_plot._canvas.axes
279  axes.cla()
280  # matplotlib
281  # concatenate x and y in order to sort
282  latest_msg = data_y[-1]
283  min_x = None
284  max_x = None
285  min_y = None
286  max_y = None
287  if isinstance(latest_msg, PlotData):
288  data = [latest_msg]
289  legend_size = 8
290  no_legend = False
291  elif isinstance(latest_msg, PlotDataArray):
292  data = latest_msg.data
293  legend_size = latest_msg.legend_font_size or 8
294  no_legend = latest_msg.no_legend
295  if latest_msg.min_x != latest_msg.max_x:
296  min_x = latest_msg.min_x
297  max_x = latest_msg.max_x
298  if latest_msg.min_y != latest_msg.max_y:
299  min_y = latest_msg.min_y
300  max_y = latest_msg.max_y
301  else:
302  rospy.logerr(
303  "Topic should be jsk_recognition_msgs/PlotData",
304  "or jsk_recognition_msgs/PlotDataArray")
305  for d in data:
306  self.plot_one(d, axes)
307  xs = add_list([d.xs for d in data])
308  ys = add_list([d.ys for d in data])
309  if min_x is None:
310  min_x = min(xs)
311  if max_x is None:
312  max_x = max(xs)
313  if min_y is None:
314  min_y = min(ys)
315  if max_y is None:
316  max_y = max(ys)
317  axes.set_xlim(min_x, max_x)
318  axes.set_ylim(min_y, max_y)
319  # line fitting
320  if not no_legend and not self.no_legend:
321  axes.legend(prop={'size': legend_size})
322  axes.grid()
323  if self.xtitle:
324  axes.set_xlabel(self.xtitle)
325  if self.ytitle:
326  axes.set_ylabel(self.ytitle)
327  self.data_plot._canvas.draw()
328  buffer = StringIO()
329  self.data_plot._canvas.figure.savefig(buffer, format="png")
330  buffer.seek(0)
331  img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
332  if LooseVersion(cv2.__version__).version[0] < 2:
333  iscolor = cv2.CV_LOAD_IMAGE_COLOR
334  else:
335  iscolor = cv2.IMREAD_COLOR
336  img = cv2.imdecode(img_array, iscolor)
337  self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
338 
339 
340 class MatPlot2D(QWidget):
341 
342  class Canvas(FigureCanvas):
343  def __init__(self, parent=None):
344  super(MatPlot2D.Canvas, self).__init__(Figure())
345  self.axes = self.figure.add_subplot(111)
346  self.figure.tight_layout()
347  self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
348  self.updateGeometry()
349 
350  def resizeEvent(self, event):
351  super(MatPlot2D.Canvas, self).resizeEvent(event)
352  self.figure.tight_layout()
353 
354  def __init__(self, parent=None):
355  super(MatPlot2D, self).__init__(parent)
357  self._toolbar = NavigationToolbar(self._canvas, self._canvas)
358  vbox = QVBoxLayout()
359  vbox.addWidget(self._toolbar)
360  vbox.addWidget(self._canvas)
361  self.setLayout(vbox)
362 
363  def redraw(self):
364  pass
365 
366  def clear(self):
367  self._canvas.axes.cla()
368  self._canvas.draw()
def on_pause_button_clicked(self, checked)
Definition: plot_2d.py:224
def enable_timer(self, enabled=True)
Definition: plot_2d.py:213
def subscribe_topic(self, topic_name)
Definition: plot_2d.py:198
def __init__(self, context)
Definition: plot_2d.py:103
def __init__(self, parent=None)
Definition: plot_2d.py:354
def __init__(self, parent=None)
Definition: plot_2d.py:343
def plot_one(self, msg, axes)
Definition: plot_2d.py:227
def _parse_args(self, argv)
Definition: plot_2d.py:119


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