class PlotWidget(QWidget): _redraw_interval = 40 def __init__(self, node, initial_topics=None, start_paused=False): super(PlotWidget, self).__init__() self.setObjectName('PlotWidget') self._node = node self._initial_topics = initial_topics _, package_path = get_resource('packages', 'rqt_plot') ui_file = os.path.join(package_path, 'share', 'rqt_plot', 'resource', 'plot.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add')) self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = None self.subscribe_topic_button.setEnabled(False) if start_paused: self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics(node) self.topic_edit.setCompleter(self._topic_completer) self._start_time = time.time() self._rosdata = {} self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) def switch_data_plot_widget(self, data_plot): self.enable_timer(enabled=False) self.data_plot_layout.removeWidget(self.data_plot) if self.data_plot is not None: self.data_plot.close() self.data_plot = data_plot self.data_plot_layout.addWidget(self.data_plot) self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) # setup drag 'n drop self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None else: for topic_name, rosdata in self._rosdata.items(): data_x, data_y = rosdata.next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or \ len(event.source().selectedItems()) == 0: qWarning( 'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or ' 'len(event.source().selectedItems()) == 0') return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning( 'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)' ) return else: topic_name = str(event.mimeData().text()) # check for plottable field type plottable, message = is_plottable(self._node, topic_name) if plottable: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics(self._node) plottable, message = is_plottable(self._node, topic_name) self.subscribe_topic_button.setEnabled(plottable) self.subscribe_topic_button.setToolTip(message) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.add_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot(bool) def on_autoscroll_checkbox_clicked(self, checked): self.data_plot.autoscroll(checked) if checked: self.data_plot.redraw() @Slot() def on_clear_button_clicked(self): self.clear_plot() def update_plot(self): if self.data_plot is not None: needs_redraw = False for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() if data_x or data_y: self.data_plot.update_values(topic_name, data_x, data_y) needs_redraw = True except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) if needs_redraw: self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer(self._rosdata) self.data_plot.redraw() def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) if len(self._rosdata) > 1: all_action = QAction('All', self._remove_topic_menu) all_action.triggered.connect(self.clean_up_subscribers) self._remove_topic_menu.addAction(all_action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): topics_changed = False for topic_name in get_plot_fields(self._node, topic_name)[0]: if topic_name in self._rosdata: qWarning( 'PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) continue self._rosdata[topic_name] = ROSData(self._node, topic_name, self._start_time) if self._rosdata[topic_name].error is not None: qWarning(str(self._rosdata[topic_name].error)) del self._rosdata[topic_name] else: data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) topics_changed = True if topics_changed: self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clear_plot(self): for topic_name, _ in self._rosdata.items(): self.data_plot.clear_values(topic_name) self.data_plot.redraw() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class PlotWidget(QWidget): _interval = 500 def __init__(self, initial_topics=None, start_paused=False): super(PlotWidget, self).__init__() self.setObjectName('PlotWidget') self._initial_topics = initial_topics rp = rospkg.RosPack() ui_file = '/home/mohammad/Documents/CMU/RED/catkin_ws/src/rqt_safeguard/resource/plot.ui' loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add')) self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) # self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) # self.data_plot = None self.subscribe_topic_button.setEnabled(False) if start_paused: self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._remove_topic_menu = QMenu() self.color_box.setStyleSheet("background-color: red;") # init and start update timer for plot self._update_timer = QTimer(self) self._update_timer.timeout.connect(self.update) # def switch_data_plot_widget(self, data_plot): # self.enable_timer(enabled=False) # self.data_plot_layout.removeWidget(self.data_plot) # if self.data_plot is not None: # self.data_plot.close() # self.data_plot = data_plot # self.data_plot_layout.addWidget(self.data_plot) # self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) # # setup drag 'n drop # self.data_plot.dropEvent = self.dropEvent # self.data_plot.dragEnterEvent = self.dragEnterEvent # if self._initial_topics: # for topic_name in self._initial_topics: # self.add_topic(topic_name) # self._initial_topics = None # else: # for topic_name, rosdata in self._rosdata.items(): # data_x, data_y = rosdata.next() # self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) # self._subscribed_topics_changed() @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or len( event.source().selectedItems()) == 0: qWarning( 'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0' ) return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning( 'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)' ) return else: topic_name = str(event.mimeData().text()) # check for plottable field type plottable, message = is_plottable(topic_name) if plottable: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() plottable, message = is_plottable(topic_name) self.subscribe_topic_button.setEnabled(plottable) self.subscribe_topic_button.setToolTip(message) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.add_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) print("pause") @Slot(bool) def on_autoscroll_checkbox_clicked(self, checked): # self.data_plot.autoscroll(checked) if checked: # self.data_plot.redraw() print("checked is clicked") # @Slot() # def on_clear_button_clicked(self): # print("Clear Button is clicked") # #self.clear_plot() def update(self): print("update") for topic_name, rosdata in self._rosdata.items(): decision, oriX = rosdata.next_dx() if oriX: self.oriX.setText(str(round(oriX, 2))) oriY, oriZ = rosdata.next_yz() if oriY: self.oriY.setText(str(round(oriY, 2))) if oriZ: self.oriZ.setText(str(round(oriZ, 2))) placX, placY = rosdata.next_pxy() if placX: self.placX.setText(str(round(100 * placX, 2))) if placY: self.placY.setText(str(round(100 * placY, 2))) placZ = rosdata.next_pz() if placZ: self.placZ.setText(str(round(100 * placZ, 2))) if decision: print(decision) self.color_box.setStyleSheet( "background-color: red; color: white; font: bold; font-size: 96px;" ) self.color_box.setText("Not Safe") else: print(decision) self.color_box.setStyleSheet( "background-color: green; color: white; font: bold; font-size: 96px;" ) self.color_box.setText("Safe") # if self.data_plot is not None: # needs_redraw = False # for topic_name, rosdata in self._rosdata.items(): # try: # data_x, data_y = rosdata.next() # if data_x or data_y: # self.data_plot.update_values(topic_name, data_x, data_y) # needs_redraw = True # except RosPlotException as e: # qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) # if needs_redraw: # self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer() print("if not self.pause_button.isChecked(): IN WIDGET PY") #self.data_plot.redraw() def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) if len(self._rosdata) > 1: all_action = QAction('All', self._remove_topic_menu) all_action.triggered.connect(self.clean_up_subscribers) self._remove_topic_menu.addAction(all_action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): self.enable_timer() topics_changed = False for topic_name in get_plot_fields(topic_name)[0]: if topic_name in self._rosdata: qWarning( 'PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) continue self._rosdata[topic_name] = ROSData(topic_name, self._start_time) if self._rosdata[topic_name].error is not None: qWarning(str(self._rosdata[topic_name].error)) del self._rosdata[topic_name] else: # data_x, data_y = self._rosdata[topic_name].next() INJA TAGHIR BEDE UPDATE KON MAGHADIR RO #self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) topics_changed = True if topics_changed: self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] # self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() # def clear_plot(self): # for topic_name, _ in self._rosdata.items(): # self.data_plot.clear_values(topic_name) # self.data_plot.redraw() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() # self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_timer.start(self._interval) else: self._update_timer.stop() def alaki(self): print("alaki")
class Plot2DWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName('Plot2DWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort concatenated_data = zip(data_y[-1].xs, data_y[-1].ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line: axes.plot(xs, ys, label=self.topic_with_field_name) else: axes.scatter(xs, ys) # set limit axes.set_xlim(min(xs), max(xs)) axes.set_ylim(min(ys), max(ys)) # line fitting if self.fit_line: X = np.array(data_y[-1].xs) Y = np.array(data_y[-1].ys) A = np.array([X, np.ones(len(X))]) A = A.T a, b = np.linalg.lstsq(A, Y)[0] axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b)) if self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier) X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1)) Y = np.array(data_y[-1].ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) axes.plot(line_X, line_y_ransac, "r--", label="{0} x + {1}".format( model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0])) if not self.no_legend: axes.legend(prop={'size': '8'}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image, queue_size=1) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() if isinstance(data_y[-1], HistogramWithRange): xs = [y.count for y in data_y[-1].bins] pos = [y.min_value for y in data_y[-1].bins] widths = [y.max_value - y.min_value for y in data_y[-1].bins] axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1]) elif isinstance(data_y[-1], collections.Sequence): xs = data_y[-1] pos = np.arange(len(xs)) widths = [1] * len(xs) axes.set_xlim(xmin=0, xmax=len(xs)) else: rospy.logerr( "Topic/Field name '%s' has unsupported '%s' type." "List of float values and " "jsk_recognition_msgs/HistogramWithRange are supported." % (self.topic_with_field_name, self._rosdata.sub.data_class)) return # axes.xticks(range(5)) for p, x, w in zip(pos, xs, widths): axes.bar(p, x, color='r', align='center', width=w) axes.legend([self.topic_with_field_name], prop={'size': '8'}) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) if LooseVersion(cv2.__version__).version[0] < 3: iscolor = cv2.CV_LOAD_IMAGE_COLOR else: iscolor = cv2.IMREAD_COLOR img = cv2.imdecode(img_array, iscolor) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Plot2DWidget(QWidget): _redraw_interval = 10 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName('Plot2DWidget') rp = rospkg.RosPack() ui_file = os.path.join( rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): "callback function when form is entered" if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): rospy.loginfo("subscribe topic") self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher( topic_name + "/plot_image", Image, queue_size=1) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def plot_one(self, msg, axes): concatenated_data = zip(msg.xs, msg.ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line or msg.type is PlotData.LINE: axes.plot(xs, ys, label=msg.label or self.topic_with_field_name) else: axes.scatter(xs, ys, label=msg.label or self.topic_with_field_name) if msg.fit_line or self.fit_line: X = np.array(msg.xs) Y = np.array(msg.ys) A = np.array([X, np.ones(len(X))]) A = A.T a, b = np.linalg.lstsq(A, Y, rcond=-1)[0] axes.plot(X, (a*X+b), "g--", label="{0} x + {1}".format(a, b)) if msg.fit_line_ransac or self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier) X = np.array(msg.xs).reshape((len(msg.xs), 1)) Y = np.array(msg.ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) if len(model_ransac.estimator_.coef_) == 1: coef = model_ransac.estimator_.coef_[0] else: coef = model_ransac.estimator_.coef_[0][0] if not isinstance(model_ransac.estimator_.intercept_, list): intercept = model_ransac.estimator_.intercept_ else: intercept = model_ransac.estimator_.intercept_[0] axes.plot( line_X, line_y_ransac, "r--", label="{0} x + {1}".format(coef, intercept)) def update_plot(self): if not self._rosdata: return try: data_x, data_y = self._rosdata.next() except RosPlotException as e: rospy.logerr("Exception in subscribing topic") rospy.logerr(e.message) return if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort latest_msg = data_y[-1] min_x = None max_x = None min_y = None max_y = None if isinstance(latest_msg, PlotData): data = [latest_msg] legend_size = 8 no_legend = False elif isinstance(latest_msg, PlotDataArray): data = latest_msg.data legend_size = latest_msg.legend_font_size or 8 no_legend = latest_msg.no_legend if latest_msg.min_x != latest_msg.max_x: min_x = latest_msg.min_x max_x = latest_msg.max_x if latest_msg.min_y != latest_msg.max_y: min_y = latest_msg.min_y max_y = latest_msg.max_y else: rospy.logerr( "Topic should be jsk_recognition_msgs/PlotData", "or jsk_recognition_msgs/PlotDataArray") for d in data: self.plot_one(d, axes) xs = add_list([d.xs for d in data]) ys = add_list([d.ys for d in data]) if min_x is None: min_x = min(xs) if max_x is None: max_x = max(xs) if min_y is None: min_y = min(ys) if max_y is None: max_y = max(ys) axes.set_xlim(min_x, max_x) axes.set_ylim(min_y, max_y) # line fitting if not no_legend and not self.no_legend: axes.legend(prop={'size': legend_size}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) if LooseVersion(cv2.__version__).version[0] < 2: iscolor = cv2.CV_LOAD_IMAGE_COLOR else: iscolor = cv2.IMREAD_COLOR img = cv2.imdecode(img_array, iscolor) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Plot2DWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName("Plot2DWidget") rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path("jsk_rqt_plugins"), "resource", "plot_histogram.ui") loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme("add")) self.pause_button.setIcon(QIcon.fromTheme("media-playback-pause")) self.clear_button.setIcon(QIcon.fromTheme("edit-clear")) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot("QDropEvent*") def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort concatenated_data = zip(data_y[-1].xs, data_y[-1].ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line: axes.plot(xs, ys, label=self.topic_with_field_name) else: axes.scatter(xs, ys) # set limit axes.set_xlim(min(xs), max(xs)) axes.set_ylim(min(ys), max(ys)) # line fitting if self.fit_line: X = np.array(data_y[-1].xs) Y = np.array(data_y[-1].ys) A = np.array([X, np.ones(len(X))]) A = A.T a, b = np.linalg.lstsq(A, Y)[0] axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b)) if self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier ) X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1)) Y = np.array(data_y[-1].ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) axes.plot( line_X, line_y_ransac, "r--", label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0]), ) if not self.no_legend: axes.legend(prop={"size": "8"}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class PlotWidget(QWidget): _redraw_interval = 40 def __init__(self, arguments=None, initial_topics=None): super(PlotWidget, self).__init__() self.setObjectName('PlotWidget') ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'plot.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.remove_topic_button.setIcon(QIcon.fromTheme('remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = None self.subscribe_topic_button.setEnabled(False) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._ordered_topics = [] self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) # save command line arguments self._arguments = arguments self._initial_topics = initial_topics def switch_data_plot_widget(self, data_plot): self.enable_timer(enabled=False) self.data_plot_layout.removeWidget(self.data_plot) if self.data_plot is not None: self.data_plot.close() self.data_plot = data_plot self.data_plot_layout.addWidget(self.data_plot) # setup drag 'n drop self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None else: for topic_name, rosdata in self._rosdata.items(): data_x, data_y = rosdata.next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or len( event.source().selectedItems()) == 0: qWarning( 'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0' ) return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning( 'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)' ) return else: topic_name = str(event.mimeData().text()) # check for numeric field type is_numeric, is_array, message = is_slot_numeric(topic_name) if is_numeric and not is_array: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() is_numeric, is_array, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric and not is_array) self.subscribe_topic_button.setToolTip(message) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot() def on_mass_button_clicked(self): topics = str(self.mass_edit.toPlainText()).split() for topic in topics: self.add_topic(topic) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() def update_plot(self): if self.data_plot is not None: for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() self.data_plot.update_values(topic_name, data_x, data_y) except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if self._arguments: if self._arguments.start_paused: self.pause_button.setChecked(True) if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer(self._rosdata) def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) return self._ordered_topics.append(topic_name) self._rosdata[topic_name] = ROSData(topic_name, self._start_time) data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self._ordered_topics.remove(topic_name) self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._ordered_topics = [] self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class Plot3DWidget(QWidget): _redraw_interval = 40 def __init__(self, initial_topics=None, start_paused=False, buffer_length=100, use_poly=True, no_legend=False): super(Plot3DWidget, self).__init__() self.setObjectName('Plot3DWidget') self._buffer_length = buffer_length self._initial_topics = initial_topics rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot3d.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.remove_topic_button.setIcon(QIcon.fromTheme('remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatDataPlot3D(self, self._buffer_length, use_poly, no_legend) self.data_plot_layout.addWidget(self.data_plot) self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self.subscribe_topic_button.setEnabled(False) if start_paused: self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0: qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0') return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)') return else: topic_name = str(event.mimeData().text()) # check for numeric field type is_numeric, is_array, message = is_slot_numeric(topic_name) if is_numeric and not is_array: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() is_numeric, is_array, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric and not is_array) self.subscribe_topic_button.setToolTip(message) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.add_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot(bool) def on_autoscroll_checkbox_clicked(self, checked): self.data_plot.autoscroll(checked) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() def update_plot(self): if self.data_plot is not None: needs_redraw = False for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() if data_x or data_y: self.data_plot.update_values(topic_name, data_x, data_y) needs_redraw = True except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) if needs_redraw: self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer(self._rosdata) def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) return self._rosdata[topic_name] = ROSData(topic_name, self._start_time) if self._rosdata[topic_name].error is not None: qWarning(str(self._rosdata[topic_name].error)) del self._rosdata[topic_name] else: data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class Plot3DWidget(QWidget): _redraw_interval = 40 def __init__(self, initial_topics=None, start_paused=False, buffer_length=100, use_poly=True, no_legend=False): super(Plot3DWidget, self).__init__() self.setObjectName('Plot3DWidget') self._buffer_length = buffer_length self._initial_topics = initial_topics rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot3d.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.remove_topic_button.setIcon(QIcon.fromTheme('remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatDataPlot3D(self, self._buffer_length, use_poly, no_legend) self.data_plot_layout.addWidget(self.data_plot) self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self.subscribe_topic_button.setEnabled(False) if start_paused: self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or \ len(event.source().selectedItems()) == 0: qWarning( 'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0' ) # NOQA return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning( 'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)' ) # NOQA return else: topic_name = str(event.mimeData().text()) # check for numeric field type is_numeric, is_array, message = is_slot_numeric(topic_name) if is_numeric and not is_array: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() is_numeric, is_array, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric and not is_array) self.subscribe_topic_button.setToolTip(message) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.add_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot(bool) def on_autoscroll_checkbox_clicked(self, checked): self.data_plot.autoscroll(checked) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() def update_plot(self): if self.data_plot is not None: needs_redraw = False for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() if data_x or data_y: self.data_plot.update_values(topic_name, data_x, data_y) needs_redraw = True except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) if needs_redraw: self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if not self.pause_button.isChecked(): # if pause button is not pressed, # enable timer based on subscribed topics self.enable_timer(self._rosdata) def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) # NOQA return self._rosdata[topic_name] = ROSData(topic_name, self._start_time) if self._rosdata[topic_name].error is not None: qWarning(str(self._rosdata[topic_name].error)) del self._rosdata[topic_name] else: data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() if self._rosdata.sub.data_class is HistogramWithRange: xs = [y.count for y in data_y[-1].bins] pos = [y.min_value for y in data_y[-1].bins] widths = [y.max_value - y.min_value for y in data_y[-1].bins] axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1]) else: xs = data_y[-1] pos = np.arange(len(xs)) widths = [1] * len(xs) axes.set_xlim(xmin=0, xmax=len(xs)) #axes.xticks(range(5)) for p, x, w in zip(pos, xs, widths): axes.bar(p, x, color='r', align='center', width=w) axes.legend([self.topic_with_field_name], prop={'size': '8'}) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Plot2DWidget(QWidget): _redraw_interval = 10 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName('Plot2DWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): "callback function when form is entered" if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): rospy.loginfo("subscribe topic") self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/plot_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def plot_one(self, msg, axes): concatenated_data = zip(msg.xs, msg.ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line or msg.type is PlotData.LINE: axes.plot(xs, ys, label=msg.label or self.topic_with_field_name) else: axes.scatter(xs, ys, label=msg.label or self.topic_with_field_name) if msg.fit_line or self.fit_line: X = np.array(msg.xs) Y = np.array(msg.ys) A = np.array([X,np.ones(len(X))]) A = A.T a,b = np.linalg.lstsq(A,Y)[0] axes.plot(X,(a*X+b),"g--", label="{0} x + {1}".format(a, b)) if msg.fit_line_ransac or self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier) X = np.array(msg.xs).reshape((len(msg.xs), 1)) Y = np.array(msg.ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) axes.plot(line_X, line_y_ransac, "r--", label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0])) def update_plot(self): if not self._rosdata: return try: data_x, data_y = self._rosdata.next() except RosPlotException, e: rospy.logerr("Exception in subscribing topic") rospy.logerr(e.message) return if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort latest_msg = data_y[-1] min_x = None max_x = None min_y = None max_y = None if isinstance(latest_msg, PlotData): data = [latest_msg] legend_size = 8 no_legend = False elif isinstance(latest_msg, PlotDataArray): data = latest_msg.data legend_size = latest_msg.legend_font_size or 8 no_legend = latest_msg.no_legend if latest_msg.min_x != latest_msg.max_x: min_x = latest_msg.min_x max_x = latest_msg.max_x if latest_msg.min_y != latest_msg.max_y: min_y = latest_msg.min_y max_y = latest_msg.max_y else: rospy.logerr("Topic should be jsk_recognition_msgs/PlotData or jsk_recognition_msgs/PlotDataArray") for d in data: self.plot_one(d, axes) xs = add_list([d.xs for d in data]) ys = add_list([d.ys for d in data]) if min_x is None: min_x = min(xs) if max_x is None: max_x = max(xs) if min_y is None: min_y = min(ys) if max_y is None: max_y = max(ys) axes.set_xlim(min_x, max_x) axes.set_ylim(min_y, max_y) # line fitting if not no_legend and not self.no_legend: axes.legend(prop={'size': legend_size}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): print "hello" if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return xs = data_y[-1] axes = self.data_plot._canvas.axes axes.cla() axes.set_xlim(xmin=0, xmax=len(xs)) pos = np.arange(len(xs)) #axes.xticks(range(5)) for p, x in zip(pos, xs): axes.bar(p, x, color='r', align='center') self.data_plot._canvas.draw()
class MatPlotWidget(QWidget): """The actual Widget """ def __init__(self): super(MatPlotWidget, self).__init__() self.setObjectName("MatPlotWidget") ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), "MatPlot.ui") loadUi(ui_file, self, {"MatDataPlot": MatDataPlot}) self.subscribe_topic_button.setEnabled(False) self._topic_completer = TopicCompleter(self.topic_edit) self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} # setup drag 'n drop self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(40) # connect combobox self.comboBox.currentIndexChanged.connect(self.on_combo_box_changed) # params (colors) self.texts = {} self.data_plot._colors = {} self.sub_topics = {} n = 0 for tag in rospy.get_param("/tags"): self.texts[tag] = rospy.get_param("/" + tag + "_text") color = [float(i) for i in list(rospy.get_param("/" + tag + "_color").split(","))] self.data_plot._colors[self.texts[tag]] = color if n != 0: # dont have a topic for the reference self.sub_topics[tag] = "/error_mags/" + rospy.get_param("/tags")[0] + "_ref/" + tag + "_tgt/mag_horiz" else: n += 1 # start with subscription to filtered self.add_topic("/error_mags/rtk_ref/flt_tgt/mag_horiz") def update_plot(self): for topic_name, rosdata in self._rosdata.items(): data_x, data_y = rosdata.next() self.data_plot.update_value(topic_name, data_x, data_y) self.data_plot.draw_plot() @Slot("QDragEnterEvent*") def dragEnterEvent(self, event): if not event.mimeData().hasText(): if not hasattr(event.source(), "selectedItems") or len(event.source().selectedItems()) == 0: qWarning( "MatPlot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0" ) return item = event.source().selectedItems()[0] ros_topic_name = item.data(0, Qt.UserRole) if ros_topic_name == None: qWarning("MatPlot.dragEnterEvent(): not hasattr(item, ros_topic_name_)") return # get topic name if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) # check for numeric field type is_numeric, message = is_slot_numeric(topic_name) if is_numeric: event.acceptProposedAction() else: qWarning('MatPlot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot("QDropEvent*") def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ("", "/"): self._topic_completer.update_topics() is_numeric, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric) self.subscribe_topic_button.setToolTip(message) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning("MatPlot.add_topic(): topic already subscribed: %s" % topic_name) return self._rosdata[topic_name] = ROSData(topic_name, self._start_time) data_x, data_y = self._rosdata[topic_name].next() color = self.data_plot._colors[self.comboBox.currentText()] self.data_plot.add_curve(topic_name, data_x, data_y, color) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() @Slot(bool) def on_pause_button_clicked(self, checked): if checked: self._update_plot_timer.stop() else: self._update_plot_timer.start(40) def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} @Slot(str) def on_combo_box_changed(self, text): # print('In on_combo_box_changed') self.on_clear_button_clicked() self.data_plot.tgt_name = self.comboBox.currentText() tag = [tg for tg, txt in self.texts.iteritems() if txt == self.comboBox.currentText()][0] self.add_topic(self.sub_topics[tag]) window_title = " ".join( [ "Ground Plane Error Magnitude of Sensor:", self.comboBox.currentText(), "for Reference:", self.data_plot.ref_name, ] ) self.setWindowTitle(QApplication.translate("MatPlotWidget", window_title, None, QApplication.UnicodeUTF8))