7 from cStringIO
import StringIO
9 from cv_bridge
import CvBridge
10 from distutils.version
import LooseVersion
11 from matplotlib.figure
import Figure
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
25 from sensor_msgs.msg
import Image
26 from sklearn
import linear_model
28 from jsk_recognition_msgs.msg
import PlotData
29 from jsk_recognition_msgs.msg
import PlotDataArray
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
37 from matplotlib.backends.backend_qt5agg
import FigureCanvasQTAgg \
42 sys.modules[
'_thread'] = thread
43 from matplotlib.backends.backend_qt5agg
import FigureCanvasQTAgg \
46 from matplotlib.backends.backend_qt5agg
import NavigationToolbar2QTAgg \
49 from matplotlib.backends.backend_qt5agg
import NavigationToolbar2QT \
52 from python_qt_binding.QtGui
import QSizePolicy
53 from python_qt_binding.QtGui
import QVBoxLayout
54 from python_qt_binding.QtGui
import QWidget
56 from matplotlib.backends.backend_qt4agg
import FigureCanvasQTAgg \
61 sys.modules[
'_thread'] = thread
62 from matplotlib.backends.backend_qt4agg
import FigureCanvasQTAgg \
65 from matplotlib.backends.backend_qt4agg
import NavigationToolbar2QTAgg \
68 from matplotlib.backends.backend_qt4agg
import NavigationToolbar2QT \
77 if not self.field_evals:
79 for f
in self.field_evals:
84 "{0} index error for: {1}".format(
85 self.name, str(val).replace(
'\n',
', ')))
88 "{0} value was not numeric: {1}".format(
94 "xss = [[0, 1, 2, ...], [0, 1, 2, ...], ...]" 104 super(Plot2D, self).
__init__(context)
105 self.setObjectName(
'Plot2D')
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)
120 parser = argparse.ArgumentParser(
121 prog=
'rqt_histogram_plot', add_help=
False)
122 Plot2D.add_arguments(parser)
123 args = parser.parse_args(argv)
128 group = parser.add_argument_group(
129 'Options for rqt_histogram plugin')
131 'topics', nargs=
'?', default=[], help=
'Topics to plot')
133 '--line', action=
"store_true",
134 help=
"Plot with lines instead of scatter")
136 '--fit-line', action=
"store_true",
137 help=
"Plot line with least-square fitting")
139 '--fit-line-ransac', action=
"store_true",
140 help=
"Plot line with RANSAC")
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")
151 _redraw_interval = 10
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)
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'))
165 self.data_plot_layout.addWidget(self.
data_plot)
167 self._topic_completer.update_topics()
169 self.data_plot.dropEvent = self.
dropEvent 170 self.data_plot.dragEnterEvent = self.dragEnterEvent
176 self._update_plot_timer.timeout.connect(self.
update_plot)
181 if event.mimeData().hasText():
182 topic_name = str(event.mimeData().text())
184 droped_item = event.source().selectedItems()[0]
185 topic_name = str(droped_item.data(0, Qt.UserRole))
190 "callback function when form is entered" 191 if self.subscribe_topic_button.isEnabled():
199 rospy.loginfo(
"subscribe topic")
202 topic_name +
"/plot_image", Image, queue_size=1)
207 self._rosdata.close()
208 self.data_plot.clear()
211 rospy.logwarn(
"%s is already subscribed", topic_name)
217 self._update_plot_timer.stop()
221 self.data_plot.clear()
228 concatenated_data = zip(msg.xs, msg.ys)
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:
239 if msg.fit_line
or self.fit_line:
242 A = np.array([X, np.ones(len(X))])
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))
252 model_ransac.fit(X, Y)
254 line_y_ransac = model_ransac.predict(line_X)
255 if len(model_ransac.estimator_.coef_) == 1:
256 coef = model_ransac.estimator_.coef_[0]
258 coef = model_ransac.estimator_.coef_[0][0]
259 if not isinstance(model_ransac.estimator_.intercept_, list):
260 intercept = model_ransac.estimator_.intercept_
262 intercept = model_ransac.estimator_.intercept_[0]
264 line_X, line_y_ransac,
"r--",
265 label=
"{0} x + {1}".format(coef, intercept))
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)
278 axes = self.data_plot._canvas.axes
282 latest_msg = data_y[-1]
287 if isinstance(latest_msg, PlotData):
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
303 "Topic should be jsk_recognition_msgs/PlotData",
304 "or jsk_recognition_msgs/PlotDataArray")
317 axes.set_xlim(min_x, max_x)
318 axes.set_ylim(min_y, max_y)
320 if not no_legend
and not self.no_legend:
321 axes.legend(prop={
'size': legend_size})
324 axes.set_xlabel(self.xtitle)
326 axes.set_ylabel(self.ytitle)
327 self.data_plot._canvas.draw()
329 self.data_plot._canvas.figure.savefig(buffer, format=
"png")
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
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"))
345 self.
axes = self.figure.add_subplot(111)
346 self.figure.tight_layout()
347 self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
348 self.updateGeometry()
352 self.figure.tight_layout()
355 super(MatPlot2D, self).
__init__(parent)
367 self._canvas.axes.cla()
def resizeEvent(self, event)
def __init__(self, context)
def __init__(self, parent=None)
def add_arguments(parser)
def __init__(self, parent=None)
def _parse_args(self, argv)