From 5541bbcf70fd462ff36dd8528c487623f79bab9f Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Wed, 25 Mar 2020 14:04:53 -0700 Subject: [PATCH 1/7] nicer look --- resource/plot.ui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resource/plot.ui b/resource/plot.ui index 8419a72..cac6b66 100644 --- a/resource/plot.ui +++ b/resource/plot.ui @@ -34,7 +34,7 @@ - Topic + Topic: From c858a45b6902df5eaf105e9e298d26a303f02cff Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Fri, 3 Apr 2020 12:01:40 -0700 Subject: [PATCH 2/7] Revert to ros1 and re-implement _parse_type, _get_topic_type and get_plot_fields --- src/rqt_plot/plot_widget.py | 118 ++++++++++++++++++++---------------- src/rqt_plot/rosplot.py | 82 +++++++++++++++++++------ 2 files changed, 127 insertions(+), 73 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 98f4319..1280a64 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -33,6 +33,8 @@ 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 @@ -40,85 +42,88 @@ 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 + +from rqt_plot.rosplot import ROSData, RosPlotException, get_topic_type + +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, true, 3 + sequence -> 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 + if topic_type_info.is_bounded_array: + array_size = topic_type_info.bounded_array_size + if 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 - - slot_type, is_array, array_size = _parse_type(topic_type_str) + slot_type, is_array, array_size = _parse_type(topic_type) 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 + # 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 = _parse_type(field) + except roslib.msgs.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 not hasattr(field_class, "get_fields_and_field_types"): + msg = "Invalid field path %s in topic %s" % (field_name, real_topic) + return [], msg + + 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 - 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_type_class(slot_type) + # TODO: add bytes to this as you could treat bytes as an array of uint if field_class in (int, float, bool): topic_kind = 'boolean' if field_class == bool else 'numeric' if is_array: @@ -132,11 +137,13 @@ def get_plot_fields(node, topic_name): message = "topic %s is %s" % (topic_name, topic_kind) return [topic_name], message 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_type_class(slot_type) if slot_class in (int, float) and not is_array: numeric_fields.append(slot) message = "" @@ -323,7 +330,12 @@ def make_remove_topic_function(x): def add_topic(self, topic_name): topics_changed = False - for topic_name in get_plot_fields(self._node, topic_name)[0]: + topics, msg = get_plot_fields(self._node, topic_name) + 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 diff --git a/src/rqt_plot/rosplot.py b/src/rqt_plot/rosplot.py index 73fce85..07769ad 100644 --- a/src/rqt_plot/rosplot.py +++ b/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): """ From 686b48259dedd9c4e3d3882480344432233ebe03 Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Wed, 8 Apr 2020 01:38:40 -0700 Subject: [PATCH 3/7] updating usage of rqt_py_common to reflect newest changes --- src/rqt_plot/plot_widget.py | 38 +++++++++++++++++++++++++------------ 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 1280a64..3209eaf 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -46,6 +46,9 @@ 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 @@ -77,10 +80,10 @@ def _parse_type(topic_type_str): # -> Tuple[str, bool, int]: if topic_type_info.is_static_array: array_size = topic_type_info.static_array_size - if topic_type_info.is_bounded_array: + elif topic_type_info.is_bounded_array: array_size = topic_type_info.bounded_array_size - if topic_type_info.is_unbounded_array: + elif topic_type_info.is_unbounded_array: array_size = None return slot_type, is_array, array_size @@ -92,19 +95,24 @@ def get_plot_fields(node, topic_name): return [], message field_name = topic_name[len(real_topic) + 1:] - slot_type, is_array, array_size = _parse_type(topic_type) + is_array = False + array_size = None + slot_type = topic_type + 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 try: - field, _, field_index = _parse_type(field) - except roslib.msgs.MsgSpecException: + field, _, field_index = \ + message_field_type_helpers.separate_field_from_array_information(field) + except MsgSpecException: message = "invalid field %s in topic %s" % (field, real_topic) return [], message @@ -119,7 +127,7 @@ def get_plot_fields(node, topic_name): 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 field_class = message_field_type_helpers.get_type_class(slot_type) @@ -128,14 +136,20 @@ def get_plot_fields(node, topic_name): 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 + msg = "topic %s is fixed-size %s array" % (topic_name, topic_kind) + return ["%s[%d]" % (topic_name, i) for i in range(array_size)], 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 message_field_type_helpers.is_primitive_type(slot_type): numeric_fields = [] From 9584ff0124da04bf4d1f725afcc8d9defe85ba6a Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Wed, 8 Apr 2020 21:32:34 -0700 Subject: [PATCH 4/7] remove bytes todo. No reason to implement this --- src/rqt_plot/plot_widget.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 3209eaf..fcb5ae4 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -131,7 +131,6 @@ def get_plot_fields(node, topic_name): field_class = message_field_type_helpers.get_type_class(slot_type) - # TODO: add bytes to this as you could treat bytes as an array of uint if field_class in (int, float, bool): topic_kind = 'boolean' if field_class == bool else 'numeric' if is_array: From 0273887d1912ec47efda88a100decf5fe084deba Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Mon, 13 Apr 2020 18:04:57 -0700 Subject: [PATCH 5/7] update rqt_py_common usage --- src/rqt_plot/plot_widget.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index fcb5ae4..42e23db 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -116,10 +116,12 @@ def get_plot_fields(node, topic_name): message = "invalid field %s in topic %s" % (field, real_topic) return [], message + # 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) @@ -129,7 +131,8 @@ def get_plot_fields(node, topic_name): slot_type, slot_is_array, array_size = _parse_type(slot_type) is_array = slot_is_array - field_class = message_field_type_helpers.get_type_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' @@ -156,7 +159,8 @@ def get_plot_fields(node, topic_name): 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 = message_field_type_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 = "" From 47c5b8ed948435fb4d3d6c3a5f9e4ef6498b384b Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Thu, 16 Apr 2020 22:42:33 -0700 Subject: [PATCH 6/7] updating to newest version of rqt_py_common --- src/rqt_plot/plot_widget.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 42e23db..4244f71 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -42,7 +42,7 @@ from python_qt_binding.QtWidgets import QAction, QMenu, QWidget from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common import message_helpers, message_field_type_helpers +from rqt_py_common import message_helpers, message_field_type_helpers, topic_helpers from rqt_plot.rosplot import ROSData, RosPlotException, get_topic_type @@ -111,7 +111,7 @@ def get_plot_fields(node, topic_name): # parse the field name for an array index try: field, _, field_index = \ - message_field_type_helpers.separate_field_from_array_information(field) + topic_helpers.separate_field_from_array_information(field) except MsgSpecException: message = "invalid field %s in topic %s" % (field, real_topic) return [], message From dec1822280e09d1789bea0c1c68c906546db1530 Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Thu, 30 Apr 2020 16:45:18 -0700 Subject: [PATCH 7/7] fixing fixed array bug --- src/rqt_plot/plot_widget.py | 14 ++++++++++---- src/rqt_plot/rosplot.py | 2 +- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 4244f71..ef98193 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -138,8 +138,14 @@ def get_plot_fields(node, topic_name): topic_kind = 'boolean' if field_class == bool else 'numeric' if is_array: if array_size is not None: - msg = "topic %s is fixed-size %s array" % (topic_name, topic_kind) - return ["%s[%d]" % (topic_name, i) for i in range(array_size)], msg + 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: if field_index is not None: msg = "topic %s is variable-size %s array with ix %d" % ( @@ -345,9 +351,9 @@ 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 - topics, msg = get_plot_fields(self._node, topic_name) + topics, msg = get_plot_fields(self._node, field_path) if len(topics) == 0: qWarning("get_plot_fields failed with msg: %s" % msg) return diff --git a/src/rqt_plot/rosplot.py b/src/rqt_plot/rosplot.py index 07769ad..d3cf366 100644 --- a/src/rqt_plot/rosplot.py +++ b/src/rqt_plot/rosplot.py @@ -236,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: