Support bounded and unbounded arrays with rqt_plot (This should NOT be merged to crystal)

resource/plot.ui

@@ -34,7 +34,7 @@
      <item>
       <widget class="QLabel" name="label">
        <property name="text">
-        <string>Topic</string>
+        <string>Topic: </string>
        </property>
       </widget>
      </item>

src/rqt_plot/plot_widget.py

@@ -33,110 +33,140 @@
 import os
 import time
 
+from typing import Tuple, List, ClassVar
+
 from ament_index_python.resources import get_resource
 from python_qt_binding import loadUi
 from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot
 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_py_common import message_helpers, message_field_type_helpers, topic_helpers
+
+from rqt_plot.rosplot import ROSData, RosPlotException, get_topic_type
+
+class MsgSpecException(Exception):
+    pass
+
+def _parse_type(topic_type_str): # -> Tuple[str, bool, int]:
+    """
+    Parses a msg type string and returns a tuple with information the type
+
+    :returns: a Tuple with the base type of the slot as a str, a bool indicating
+        if the slot is an array and an integer if it has a static or bound size
+        or if it is unbounded, then the third value is None
+
+        Strips out any array information from the topic_type_str
 
-from rqt_plot.rosplot import ROSData, RosPlotException
+        eg:
+            sequence<int8, 3> -> int8, true, 3
+            sequence<int8>    -> int8, true, None
+            int8[3]           -> int8, true, 3
 
+    :rtype: str, bool, int
+    """
+    if not topic_type_str:
+        raise MsgSpecException("Invalid empty type")
 
-def _parse_type(topic_type_str):
     slot_type = topic_type_str
     is_array = False
     array_size = None
 
-    array_idx = topic_type_str.find('[')
-    if array_idx < 0:
-        return slot_type, False, None
+    topic_type_info = message_field_type_helpers.MessageFieldTypeInfo(topic_type_str)
+    slot_type = topic_type_info.base_type_str
+    is_array = topic_type_info.is_array
 
-    end_array_idx = topic_type_str.find(']', array_idx + 1)
-    if end_array_idx < 0:
-        return None, False, None
+    if topic_type_info.is_static_array:
+        array_size = topic_type_info.static_array_size
 
-    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
+    elif topic_type_info.is_bounded_array:
+        array_size = topic_type_info.bounded_array_size
 
+    elif topic_type_info.is_unbounded_array:
+        array_size = None
+
+    return slot_type, is_array, array_size
 
 def get_plot_fields(node, topic_name):
-    topics = node.get_topic_names_and_types()
-    real_topic = None
-    for name, topic_types in topics:
-        if name == topic_name[:len(name)]:
-            real_topic = name
-            topic_type_str = topic_types[0] if topic_types else None
-            break
-    if real_topic is None:
+    topic_type, real_topic, _ = get_topic_type(node, topic_name)
+    if topic_type is None:
         message = "topic %s does not exist" % (topic_name)
         return [], message
-
-    if topic_type_str is None:
-        message = "no topic types found for topic %s " % (topic_name)
-        return [], message
-
-    if len(topic_name) < len(real_topic) + 1:
-        message = 'no field specified in topic name "{}"'.format(topic_name)
-        return [], message
-
     field_name = topic_name[len(real_topic) + 1:]
 
-    message_class = message_helpers.get_message_class(topic_type_str)
-    if message_class is None:
-        message = 'message class "{}" is invalid'.format(topic_type_str)
-        return [], message
+    is_array = False
+    array_size = None
+    slot_type = topic_type
 
-    slot_type, is_array, array_size = _parse_type(topic_type_str)
     field_class = message_helpers.get_message_class(slot_type)
+    if field_class is None:
+        message = "type of topic %s is unknown" % (topic_name)
+        return [], message
 
+    field_index = None
+    # Go through the fields until you reach the last msg field
     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:
+        try:
+            field, _, field_index = \
+                topic_helpers.separate_field_from_array_information(field)
+        except MsgSpecException:
             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:
+        # If it is not a generated msg type
+        if not hasattr(field_class, "get_fields_and_field_types"):
+            msg = "Invalid field path %s in topic %s" % (field_name, real_topic)
+            return [], msg
+
+        # Find the field in the fields of field_class
+        fields_and_field_types = field_class.get_fields_and_field_types()
+        if field not in fields_and_field_types.keys() :
             message = "no field %s in topic %s" % (field_name, real_topic)
             return [], message
-        slot_type = field_names_and_types[field]
+
+        slot_type = fields_and_field_types[field]
         slot_type, slot_is_array, array_size = _parse_type(slot_type)
-        is_array = slot_is_array and field_index is None
+        is_array = slot_is_array
 
-        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)
+        field_class = message_field_type_helpers.get_field_python_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
+                if field_index is None:
+                    msg = "topic %s is a fixed-size %s array" % (topic_name, topic_kind)
+                    return ["%s[%d]" % (topic_name, i) for i in range(array_size)], msg
+                else:
+                    msg = "topic %s is a fixed-size %s array with ix: %s" % (
+                        topic_name, topic_kind, field_index
+                    )
+                    return [topic_name], msg
             else:
-                message = "topic %s is variable-size %s array" % (topic_name, topic_kind)
-                return [], message
+                if field_index is not None:
+                    msg = "topic %s is variable-size %s array with ix %d" % (
+                        topic_name, topic_kind, field_index
+                    )
+                    return [topic_name], msg
+                else:
+                    msg = "topic %s is variable-size %s array" % (topic_name, topic_kind)
+                    return [], msg
         else:
-            message = "topic %s is %s" % (topic_name, topic_kind)
-            return [topic_name], message
+            msg = "topic %s is %s" % (topic_name, topic_kind)
+            return [topic_name], msg
     else:
-        if not topic_helpers.is_primitive_type(slot_type):
+        if not message_field_type_helpers.is_primitive_type(slot_type):
             numeric_fields = []
-            for slot, slot_type in field_class.get_fields_and_field_types().items():
+            fields_and_field_types = field_class.get_fields_and_field_types()
+            for i, slot in enumerate(fields_and_field_types.keys()):
+                slot_type = fields_and_field_types[slot]
                 slot_type, is_array, array_size = _parse_type(slot_type)
-                slot_class = topic_helpers.get_type_class(slot_type)
+                slot_class = \
+                    message_field_type_helpers.get_field_python_class(slot_type)
                 if slot_class in (int, float) and not is_array:
                     numeric_fields.append(slot)
             message = ""
@@ -321,9 +351,14 @@ def make_remove_topic_function(x):
 
         self.remove_topic_button.setMenu(self._remove_topic_menu)
 
-    def add_topic(self, topic_name):
+    def add_topic(self, field_path):
         topics_changed = False
-        for topic_name in get_plot_fields(self._node, topic_name)[0]:
+        topics, msg = get_plot_fields(self._node, field_path)
+        if len(topics) == 0:
+            qWarning("get_plot_fields failed with msg: %s" % msg)
+            return
+
+        for topic_name in topics:
             if topic_name in self._rosdata:
                 qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name)
                 continue

src/rqt_plot/rosplot.py

@@ -32,12 +32,13 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 #
-
-import string
 import sys
+
 import threading
 import time
 
+from operator import itemgetter
+
 from rclpy.qos import QoSProfile
 from rqt_py_common.message_helpers import get_message_class
 from std_msgs.msg import Bool
@@ -47,41 +48,82 @@
 class RosPlotException(Exception):
     pass
 
+def _get_nested_attribute(msg, nested_attributes):
+    value = msg
+    for attr in nested_attributes.split('/'):
+        value = getattr(value, attr)
+    return value
 
-def _get_topic_type(node, topic):
+def _get_topic_type(topic_names_and_types, path_to_field):
     """
-    subroutine for getting the topic type
+    subroutine for getting the topic type, topic name and path to field
     (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn)
 
-    :returns: topic type, real topic name, and rest of name referenced
+    :returns: topic type, real topic name, and path_to_field
       if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
     """
-    val = node.get_topic_names_and_types()
-    matches = [(t, t_types) for t, t_types in val if t == topic or topic.startswith(t + '/')]
-    for t, t_types in matches:
-        for t_type in t_types:
-            if t_type == topic:
-                return t_type, None, None
-        for t_type in t_types:
-            if t_type != '*':
-                return t_type, t, topic[len(t):]
-    return None, None, None
+    # See if we can find a full match
+    matches = []
+    for (t_name, t_types) in topic_names_and_types:
+        if t_name == path_to_field:
+            for t_type in t_types:
+                matches.append((t_name, t_type))
+
+    if not matches:
+        for (t_name, t_types) in topic_names_and_types:
+            if path_to_field.startswith(t_name + '/'):
+                for t_type in t_types:
+                    matches.append((t_name, t_type))
+
+        # choose longest match first
+        matches.sort(key=itemgetter(0), reverse=True)
+
+        # try to ignore messages which don't have the field specified as part of the topic name
+        while matches:
+            t_name, t_type = matches[0]
+            msg_class = get_message_class(t_type)
+            if not msg_class:
+                # if any class is not fetchable skip ignoring any message types
+                break
+
+            msg = msg_class()
+            nested_attributes = path_to_field[len(t_name) + 1:].rstrip('/')
+            nested_attributes = nested_attributes.split('[')[0]
+            if nested_attributes == '':
+                break
+            try:
+                _get_nested_attribute(msg, nested_attributes)
+            except AttributeError:
+                # ignore this type since it does not have the requested field
+                matches.pop(0)
+                continue
+            # Select this match
+            matches = [(t_name, t_type)]
+            break
+    if matches:
+        t_name, t_type = matches[0]
+        # This is a relic from ros1 where rosgraph.names.ANYTYPE = '*'.
+        # TODO(remove)
+        if t_type == '*':
+            return None, None, None
+        return t_type, t_name, path_to_field[len(t_name):]
 
+    return None, None, None
 
-def get_topic_type(node, topic):
+def get_topic_type(node, path_to_field):
     """
     Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn)
 
-    :returns: topic type, real topic name, and rest of name referenced
-      if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
+    :returns: topic type, real topic name and rest of name referenced
+      if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
     """
-    topic_type, real_topic, rest = _get_topic_type(node, topic)
+    topic_names_and_types = node.get_topic_names_and_types()
+    topic_type, real_topic, rest = _get_topic_type(topic_names_and_types, path_to_field)
     if topic_type:
         return topic_type, real_topic, rest
     else:
         return None, None, None
 
-
 class ROSData(object):
 
     """
@@ -194,7 +236,7 @@ def generate_field_evals(fields):
         fields = [f for f in fields.split('/') if f]
         for f in fields:
             if '[' in f:
-                field_name, rest = f.split('[')
+                field_name, rest = f.split('[', maxsplit=1)
                 slot_num = int(rest[:rest.find(']')])
                 evals.append(_array_eval(field_name, slot_num))
             else: