From 9c56eabdb696ed1875599d2e3b975aa5000498a9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 5 Mar 2021 14:41:10 -0300 Subject: [PATCH] Fix plots of array items (#71) Signed-off-by: Ivan Santiago Paunovic --- src/rqt_plot/plot_widget.py | 171 ++++++++++++++++++++---------------- src/rqt_plot/rosplot.py | 4 +- 2 files changed, 97 insertions(+), 78 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 98f4319..1bf8253 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -31,6 +31,7 @@ # POSSIBILITY OF SUCH DAMAGE. import os +import re import time from ament_index_python.resources import get_resource @@ -39,32 +40,35 @@ from python_qt_binding.QtGui import QIcon from python_qt_binding.QtWidgets import QAction, QMenu, QWidget -from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common import topic_helpers, message_helpers - -from rqt_plot.rosplot import ROSData, RosPlotException +from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import Array +from rosidl_parser.definition import BasicType +from rosidl_parser.definition import BOOLEAN_TYPE +from rosidl_parser.definition import NamespacedType +from rosidl_runtime_py.utilities import get_message +from rosidl_runtime_py.utilities import get_message_namespaced_type +from rosidl_runtime_py import import_message_from_namespaced_type -def _parse_type(topic_type_str): - slot_type = topic_type_str - is_array = False - array_size = None +from rqt_py_common.topic_completer import TopicCompleter - array_idx = topic_type_str.find('[') - if array_idx < 0: - return slot_type, False, None +from rqt_plot.rosplot import ROSData, RosPlotException - end_array_idx = topic_type_str.find(']', array_idx + 1) - if end_array_idx < 0: - return None, False, None +ARRAY_TYPE_REGEX = re.compile(r'(.+)\[(.*)\]') - slot_type = topic_type_str[:array_idx] - array_size_str = topic_type_str[array_idx + 1 : end_array_idx] - try: - array_size = int(array_size_str) - return slot_type, True, array_size - except ValueError as e: - return slot_type, True, None +def _parse_field_name_and_index(field_name): + # Field names may be indexed, e.g. `my_field[2]`. + # This parses the actual name and index from the indexed name and returns `field_name, index`. + # If not indexed, returns `field_name, None`. + m = ARRAY_TYPE_REGEX.match(field_name) + if m: + try: + return m.group(1), int(m.group(2)) + except ValueError: + pass + return field_name, None def get_plot_fields(node, topic_name): @@ -87,67 +91,82 @@ def get_plot_fields(node, topic_name): message = 'no field specified in topic name "{}"'.format(topic_name) return [], message - field_name = topic_name[len(real_topic) + 1:] + nested_field_path = topic_name[len(real_topic) + 1:] - message_class = message_helpers.get_message_class(topic_type_str) + message_class = get_message(topic_type_str) if message_class is None: message = 'message class "{}" is invalid'.format(topic_type_str) return [], message - slot_type, is_array, array_size = _parse_type(topic_type_str) - field_class = message_helpers.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 - field, _, field_index = _parse_type(field) - if field is None: - message = "invalid field %s in topic %s" % (field, real_topic) - return [], message - - field_names_and_types = field_class.get_fields_and_field_types() - if field not in field_names_and_types: - message = "no field %s in topic %s" % (field_name, real_topic) - return [], message - slot_type = field_names_and_types[field] - slot_type, slot_is_array, array_size = _parse_type(slot_type) - is_array = slot_is_array and field_index is None + nested_fields = iter(f for f in nested_field_path.split('/') if f) + current_type = get_message_namespaced_type(topic_type_str) + current_message_class = message_class + next_field = next(nested_fields, None) + parsed_fields = [] + + while next_field is not None: + parsed_fields.append(next_field) + name, index = _parse_field_name_and_index(next_field) + has_index = index is not None + base_error_msg = f"trying to parse field '{'.'.join(parsed_fields)}' of topic {real_topic}: " + no_field_error_msg = base_error_msg + f"'{name}' is not a field of '{topic_type_str}'" + + try: + slot_index = current_message_class.__slots__.index(f'_{name}') + except ValueError: + return [], no_field_error_msg + current_type = current_message_class.SLOT_TYPES[slot_index] + is_array_or_sequence = isinstance(current_type, AbstractNestedType) + + if is_array_or_sequence: + if not has_index: + return [], base_error_msg + f'{name} is a nested type but not index provided' + if current_type.has_maximum_size(): + if index >= current_type.maximum_size: + return [], ( + base_error_msg + + f"index '{index}' out of bounds, maximum size is {current_type.maximum_size}") + current_type = current_type.value_type + elif has_index: + return [], base_error_msg + "{name} is not an array or sequence" + + if not isinstance(current_type, NamespacedType): + break + current_message_class = import_message_from_namespaced_type(current_type) + next_field = next(nested_fields, None) - if topic_helpers.is_primitive_type(slot_type): - field_class = topic_helpers.get_type_class(slot_type) - else: - field_class = message_helpers.get_message_class(slot_type) - - if field_class in (int, float, bool): - topic_kind = 'boolean' if field_class == bool else 'numeric' - if is_array: - if array_size is not None: - message = "topic %s is fixed-size %s array" % (topic_name, topic_kind) - return ["%s[%d]" % (topic_name, i) for i in range(array_size)], message - else: - message = "topic %s is variable-size %s array" % (topic_name, topic_kind) - return [], message - else: - message = "topic %s is %s" % (topic_name, topic_kind) - return [topic_name], message - else: - if not topic_helpers.is_primitive_type(slot_type): - numeric_fields = [] - for slot, slot_type in field_class.get_fields_and_field_types().items(): - slot_type, is_array, array_size = _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 + try: + next_field = next(nested_fields) + return [], f"'{'.'.join(parsed_fields)}' is a primitive type with no field named '{next_field}'" + except StopIteration: + pass + + if isinstance(current_type, AbstractGenericString): + return [], f"'{topic_name}' is a string, which cannot be plotted" + if isinstance(current_type, AbstractSequence): + return [], f"'{topic_name}' is a sequence, which cannot be plotted" + if isinstance(current_type, Array): + return ( + [f'{topic_name}[{i}]' for i in range(field_class.maximum_size)], + f"'{topic_name}' is a fixed size array") + if isinstance(current_type, NamespacedType): + plottable_fields = [] + current_message_class = import_message_from_namespaced_type(current_type) + for n_field, n_current_type in zip( + current_message_class.__slots__, current_message_class.SLOT_TYPES + ): + if isinstance(n_current_type, BasicType): + plottable_fields.append(n_field[1:]) + if plottable_fields: + return ( + [f'{topic_name}/{field}' for field in plottable_fields], + f"{len(plottable_fields)} plottable fields in '{topic_name}'" + ) + if not isinstance(current_type, BasicType): + return [], f"{topic_name} cannot be plotted" + + data_kind = 'boolean' if current_type.typename == BOOLEAN_TYPE else 'numeric' + return [topic_name], f"topic '{topic_name}' is {data_kind}" def is_plottable(node, topic_name): diff --git a/src/rqt_plot/rosplot.py b/src/rqt_plot/rosplot.py index 73fce85..a3cd88f 100644 --- a/src/rqt_plot/rosplot.py +++ b/src/rqt_plot/rosplot.py @@ -39,7 +39,7 @@ import time from rclpy.qos import QoSProfile -from rqt_py_common.message_helpers import get_message_class +from rosidl_runtime_py.utilities import get_message from std_msgs.msg import Bool from python_qt_binding.QtCore import qWarning @@ -101,7 +101,7 @@ def __init__(self, node, topic, start_time): topic_type, real_topic, fields = get_topic_type(node, topic) if topic_type is not None: self.field_evals = generate_field_evals(fields) - data_class = get_message_class(topic_type) + data_class = get_message(topic_type) self.sub = node.create_subscription( data_class, real_topic, self._ros_cb, qos_profile=QoSProfile(depth=10)) else: