From 69429c94734ed0dec8dab0e329151d2548eb78e2 Mon Sep 17 00:00:00 2001 From: trainman419 Date: Tue, 3 Jun 2014 16:08:55 -0700 Subject: [PATCH] Make more things plottable This extends the notion of what is plottable to include arrays and simple message types, in addition to individual fields in messages --- rqt_plot/src/rqt_plot/plot_widget.py | 96 +++++++++++++++---- .../src/rqt_py_common/topic_helpers.py | 21 ++-- 2 files changed, 91 insertions(+), 26 deletions(-) diff --git a/rqt_plot/src/rqt_plot/plot_widget.py b/rqt_plot/src/rqt_plot/plot_widget.py index 0a54dd56..549327fc 100644 --- a/rqt_plot/src/rqt_plot/plot_widget.py +++ b/rqt_plot/src/rqt_plot/plot_widget.py @@ -32,6 +32,7 @@ import os import rospkg +import roslib from python_qt_binding import loadUi from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot @@ -40,10 +41,68 @@ import rospy from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common.topic_helpers import is_slot_numeric +from rqt_py_common import topic_helpers from . rosplot import ROSData, RosPlotException +def get_plot_fields(topic_name): + topic_type, real_topic, _ = topic_helpers.get_topic_type(topic_name) + if topic_type is None: + message = "topic %s does not exist" % ( topic_name ) + return [], message + field_name = topic_name[len(real_topic)+1:] + + slot_type, is_array, array_size = roslib.msgs.parse_type(topic_type) + field_class = roslib.message.get_message_class(slot_type) + + fields = [f for f in field_name.split('/') if f] + + for field in fields: + # parse the field name for an array index + try: + field, _, field_index = roslib.msgs.parse_type(field) + except roslib.msgs.MsgSpecException: + message = "invalid field %s in topic %s" % ( field, real_topic ) + return [], message + + if field not in getattr(field_class, '__slots__', []): + message = "no field %s in topic %s" % ( field_name, real_topic ) + return [], message + slot_type = field_class._slot_types[field_class.__slots__.index(field)] + slot_type, slot_is_array, array_size = roslib.msgs.parse_type(slot_type) + is_array = slot_is_array and field_index is None + + field_class = topic_helpers.get_type_class(slot_type) + + if field_class in (int, float): + if is_array: + message = "topic %s is a numeric array" % ( topic_name ) + return [ "%s[%d]" % (topic_name, i) for i in range(array_size) ], message + else: + message = "topic %s is numeric" % ( topic_name ) + return [ topic_name ], message + else: + if not roslib.msgs.is_valid_constant_type(slot_type): + numeric_fields = [] + for i, slot in enumerate(field_class.__slots__): + slot_type = field_class._slot_types[i] + slot_type, is_array, array_size = roslib.msgs.parse_type(slot_type) + slot_class = topic_helpers.get_type_class(slot_type) + if slot_class in (int, float) and not is_array: + numeric_fields.append(slot) + message = "" + if len(numeric_fields) > 0: + message = "%d plottable fields in %s" % ( len(numeric_fields), topic_name ) + else: + message = "No plottable fields in %s" % ( topic_name ) + return [ "%s/%s" % (topic_name, f) for f in numeric_fields ], message + else: + message = "Topic %s is not numeric" % ( topic_name ) + return [], message + +def is_plottable(topic_name): + fields, message = get_plot_fields(topic_name) + return len(fields) > 0, message class PlotWidget(QWidget): _redraw_interval = 40 @@ -120,9 +179,9 @@ def dragEnterEvent(self, event): 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: + # check for plottable field type + plottable, message = is_plottable(topic_name) + if plottable: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @@ -142,8 +201,8 @@ def on_topic_edit_textChanged(self, topic_name): 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) + plottable, message = is_plottable(topic_name) + self.subscribe_topic_button.setEnabled(plottable) self.subscribe_topic_button.setToolTip(message) @Slot() @@ -202,18 +261,21 @@ def make_remove_topic_function(x): 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) + 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() + 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): diff --git a/rqt_py_common/src/rqt_py_common/topic_helpers.py b/rqt_py_common/src/rqt_py_common/topic_helpers.py index d0afb8b1..7e27be54 100644 --- a/rqt_py_common/src/rqt_py_common/topic_helpers.py +++ b/rqt_py_common/src/rqt_py_common/topic_helpers.py @@ -33,6 +33,16 @@ from rostopic import get_topic_type from python_qt_binding.QtCore import qDebug +def get_type_class(type_name): + if roslib.msgs.is_valid_constant_type(type_name): + if type_name == 'string': + return str + elif type_name == 'bool': + return bool + else: + return type(roslib.msgs._convert_val(type_name, 0)) + else: + return roslib.message.get_message_class(type_name) def get_field_type(topic_name): """ @@ -81,15 +91,8 @@ def get_slot_type(message_class, slot_path): slot_type = message_class._slot_types[message_class.__slots__.index(field_name)] slot_type, slot_is_array, _ = roslib.msgs.parse_type(slot_type) is_array = slot_is_array and field_index is None - if roslib.msgs.is_valid_constant_type(slot_type): - if slot_type == 'string': - message_class = str - elif slot_type == 'bool': - message_class = bool - else: - message_class = type(roslib.msgs._convert_val(slot_type, 0)) - else: - message_class = roslib.message.get_message_class(slot_type) + + message_class = get_type_class(slot_type) return message_class, is_array