hist.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import argparse
4 import collections
5 import os
6 import sys
7 
8 from cStringIO import StringIO
9 import cv2
10 from cv_bridge import CvBridge
11 from distutils.version import LooseVersion
12 from matplotlib.figure import Figure
13 import numpy as np
14 import python_qt_binding
15 from python_qt_binding import loadUi
16 from python_qt_binding.QtCore import Qt
17 from python_qt_binding.QtCore import QTimer
18 from python_qt_binding.QtCore import Slot
19 from python_qt_binding.QtGui import QIcon
20 import rospkg
21 import rospy
22 from rqt_gui_py.plugin import Plugin
23 from rqt_plot.rosplot import ROSData as _ROSData
24 from rqt_plot.rosplot import RosPlotException
25 from rqt_py_common.topic_completer import TopicCompleter
26 from sensor_msgs.msg import Image
27 
28 from jsk_recognition_msgs.msg import HistogramWithRange
29 
30 # qt5 in kinetic
31 if LooseVersion(python_qt_binding.QT_BINDING_VERSION).version[0] >= 5:
32  from python_qt_binding.QtWidgets import QSizePolicy
33  from python_qt_binding.QtWidgets import QVBoxLayout
34  from python_qt_binding.QtWidgets import QWidget
35  try:
36  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
37  as FigureCanvas
38  except ImportError:
39  # work around bug in dateutil
40  import thread
41  sys.modules['_thread'] = thread
42  from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg \
43  as FigureCanvas
44  try:
45  from matplotlib.backends.backend_qt5agg import NavigationToolbar2QTAgg \
46  as NavigationToolbar
47  except ImportError:
48  from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT \
49  as NavigationToolbar
50 else:
51  from python_qt_binding.QtGui import QSizePolicy
52  from python_qt_binding.QtGui import QVBoxLayout
53  from python_qt_binding.QtGui import QWidget
54  try:
55  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
56  as FigureCanvas
57  except ImportError:
58  # work around bug in dateutil
59  import thread
60  sys.modules['_thread'] = thread
61  from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg \
62  as FigureCanvas
63  try:
64  from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg \
65  as NavigationToolbar
66  except ImportError:
67  from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT \
68  as NavigationToolbar
69 
70 
71 class ROSData(_ROSData):
72  def _get_data(self, msg):
73  val = msg
74  try:
75  if not self.field_evals:
76  return val
77  for f in self.field_evals:
78  val = f(val)
79  return val
80  except IndexError:
82  "{0} index error for: {1}".format(
83  self.name, str(val).replace('\n', ', ')))
84  except TypeError:
85  self.error = RosPlotException(
86  "{0} value was not numeric: {1}".format(
87  self.name, val))
88 
89 
91  def __init__(self, context):
92  super(HistogramPlot, self).__init__(context)
93  self.setObjectName('HistogramPlot')
94  self._args = self._parse_args(context.argv())
95  self._widget = HistogramPlotWidget(self._args.topics)
96  context.add_widget(self._widget)
97 
98  def _parse_args(self, argv):
99  parser = argparse.ArgumentParser(
100  prog='rqt_histogram_plot', add_help=False)
101  HistogramPlot.add_arguments(parser)
102  args = parser.parse_args(argv)
103  return args
104 
105  @staticmethod
106  def add_arguments(parser):
107  group = parser.add_argument_group('Options for rqt_histogram plugin')
108  group.add_argument(
109  'topics', nargs='?', default=[], help='Topics to plot')
110 
111 
112 class HistogramPlotWidget(QWidget):
113  _redraw_interval = 40
114 
115  def __init__(self, topics):
116  super(HistogramPlotWidget, self).__init__()
117  self.setObjectName('HistogramPlotWidget')
118  rp = rospkg.RosPack()
119  ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
120  'resource', 'plot_histogram.ui')
121  loadUi(ui_file, self)
122  self.cv_bridge = CvBridge()
123  self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
124  self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
125  self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
127  self.data_plot_layout.addWidget(self.data_plot)
128  self._topic_completer = TopicCompleter(self.topic_edit)
129  self._topic_completer.update_topics()
130  self.topic_edit.setCompleter(self._topic_completer)
131  self.data_plot.dropEvent = self.dropEvent
132  self.data_plot.dragEnterEvent = self.dragEnterEvent
133  self._start_time = rospy.get_time()
134  self._rosdata = None
135  if len(topics) != 0:
136  self.subscribe_topic(topics)
137  self._update_plot_timer = QTimer(self)
138  self._update_plot_timer.timeout.connect(self.update_plot)
139  self._update_plot_timer.start(self._redraw_interval)
140 
141  @Slot('QDropEvent*')
142  def dropEvent(self, event):
143  if event.mimeData().hasText():
144  topic_name = str(event.mimeData().text())
145  else:
146  droped_item = event.source().selectedItems()[0]
147  topic_name = str(droped_item.data(0, Qt.UserRole))
148  self.subscribe_topic(topic_name)
149 
150  @Slot()
152  if self.subscribe_topic_button.isEnabled():
153  self.subscribe_topic(str(self.topic_edit.text()))
154 
155  @Slot()
157  self.subscribe_topic(str(self.topic_edit.text()))
158 
159  def subscribe_topic(self, topic_name):
160  self.topic_with_field_name = topic_name
161  self.pub_image = rospy.Publisher(
162  topic_name + "/histogram_image", Image, queue_size=1)
163  if not self._rosdata:
164  self._rosdata = ROSData(topic_name, self._start_time)
165  else:
166  if self._rosdata != topic_name:
167  self._rosdata.close()
168  self.data_plot.clear()
169  self._rosdata = ROSData(topic_name, self._start_time)
170  else:
171  rospy.logwarn("%s is already subscribed", topic_name)
172 
173  def enable_timer(self, enabled=True):
174  if enabled:
175  self._update_plot_timer.start(self._redraw_interval)
176  else:
177  self._update_plot_timer.stop()
178 
179  @Slot()
181  self.data_plot.clear()
182 
183  @Slot(bool)
184  def on_pause_button_clicked(self, checked):
185  self.enable_timer(not checked)
186 
187  def update_plot(self):
188  if not self._rosdata:
189  return
190  data_x, data_y = self._rosdata.next()
191 
192  if len(data_y) == 0:
193  return
194  axes = self.data_plot._canvas.axes
195  axes.cla()
196  if isinstance(data_y[-1], HistogramWithRange):
197  xs = [y.count for y in data_y[-1].bins]
198  pos = [y.min_value for y in data_y[-1].bins]
199  widths = [y.max_value - y.min_value for y in data_y[-1].bins]
200  axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1])
201  elif isinstance(data_y[-1], collections.Sequence):
202  xs = data_y[-1]
203  pos = np.arange(len(xs))
204  widths = [1] * len(xs)
205  axes.set_xlim(xmin=0, xmax=len(xs))
206  else:
207  rospy.logerr(
208  "Topic/Field name '%s' has unsupported '%s' type."
209  "List of float values and "
210  "jsk_recognition_msgs/HistogramWithRange are supported."
211  % (self.topic_with_field_name,
212  self._rosdata.sub.data_class))
213  return
214  # axes.xticks(range(5))
215  for p, x, w in zip(pos, xs, widths):
216  axes.bar(p, x, color='r', align='center', width=w)
217  axes.legend([self.topic_with_field_name], prop={'size': '8'})
218  self.data_plot._canvas.draw()
219  buffer = StringIO()
220  self.data_plot._canvas.figure.savefig(buffer, format="png")
221  buffer.seek(0)
222  img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
223  if LooseVersion(cv2.__version__).version[0] < 3:
224  iscolor = cv2.CV_LOAD_IMAGE_COLOR
225  else:
226  iscolor = cv2.IMREAD_COLOR
227  img = cv2.imdecode(img_array, iscolor)
228  self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
229 
230 
231 class MatHistogramPlot(QWidget):
232  class Canvas(FigureCanvas):
233  def __init__(self, parent=None):
234  super(MatHistogramPlot.Canvas, self).__init__(Figure())
235  self.axes = self.figure.add_subplot(111)
236  self.figure.tight_layout()
237  self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
238  self.updateGeometry()
239 
240  def resizeEvent(self, event):
241  super(MatHistogramPlot.Canvas, self).resizeEvent(event)
242  self.figure.tight_layout()
243 
244  def __init__(self, parent=None):
245  super(MatHistogramPlot, self).__init__(parent)
247  self._toolbar = NavigationToolbar(self._canvas, self._canvas)
248  vbox = QVBoxLayout()
249  vbox.addWidget(self._toolbar)
250  vbox.addWidget(self._canvas)
251  self.setLayout(vbox)
252 
253  def redraw(self):
254  pass
255 
256  def clear(self):
257  self._canvas.axes.cla()
258  self._canvas.draw()
def __init__(self, context)
Definition: hist.py:91
def on_pause_button_clicked(self, checked)
Definition: hist.py:184
def __init__(self, parent=None)
Definition: hist.py:244
def _parse_args(self, argv)
Definition: hist.py:98
def __init__(self, parent=None)
Definition: hist.py:233
def subscribe_topic(self, topic_name)
Definition: hist.py:159
def enable_timer(self, enabled=True)
Definition: hist.py:173


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