From 11eae39de7f0fccde799cdf3d83c1913d3c5ec04 Mon Sep 17 00:00:00 2001 From: Stephen Date: Wed, 5 Dec 2018 16:13:17 -0800 Subject: [PATCH] Porting to ros2 --- CMakeLists.txt | 20 ++++--- package.xml | 29 +++++----- scripts/rqt_plot | 2 +- src/rqt_plot/plot.py | 45 ++++++++++++--- src/rqt_plot/plot_widget.py | 110 ++++++++++++++++++++++++------------ src/rqt_plot/rosplot.py | 59 +++++++++---------- 6 files changed, 165 insertions(+), 100 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b23f304..d147f19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,18 +1,22 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(rqt_plot) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME}) install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(PROGRAMS scripts/rqt_plot - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) + +ament_package() diff --git a/package.xml b/package.xml index 4ea73af..300c4f4 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + rqt_plot 0.4.8 rqt_plot provides a GUI plugin visualizing numeric values in a 2D plot using different plotting backends. @@ -14,23 +14,22 @@ Dorian Scholz - catkin + ament_cmake - python-matplotlib - python-numpy - python-rospkg - python_qt_binding - qt_gui_py_common - qwt_dependency - rosgraph - rostopic - rqt_gui - rqt_gui_py - rqt_py_common - std_msgs + python_qt_binding + python3-matplotlib + python3-numpy + python3-catkin-pkg-modules + qt_gui_py_common + qwt_dependency + rclpy + rqt_gui + rqt_gui_py + rqt_py_common + std_msgs - + ament_cmake diff --git a/scripts/rqt_plot b/scripts/rqt_plot index 57e1225..e85b0a3 100755 --- a/scripts/rqt_plot +++ b/scripts/rqt_plot @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import sys diff --git a/src/rqt_plot/plot.py b/src/rqt_plot/plot.py index 5597fc0..9787925 100644 --- a/src/rqt_plot/plot.py +++ b/src/rqt_plot/plot.py @@ -50,10 +50,10 @@ def __init__(self, context): self.setObjectName('Plot') self._context = context - + self._node = context.node self._args = self._parse_args(context.argv()) self._widget = PlotWidget( - initial_topics=self._args.topics, start_paused=self._args.start_paused) + self._node, initial_topics=self._args.topics, start_paused=self._args.start_paused) self._data_plot = DataPlot(self._widget) # disable autoscaling of X, and set a sane default range @@ -67,6 +67,34 @@ def __init__(self, context): self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) + def _resolve_topic_name(script_name, name): + """ + Name resolver for scripts. + + Derived from + https://github.com/ros/ros_comm/blob/melodic-devel/tools/rosgraph/src/rosgraph/names.py + + :param name: name to resolve, ``str`` + :param script_name: name of script. script_name must not + contain a namespace., ``str`` + :returns: resolved name, ``str`` + """ + sep = '/' + priv_name = '~' + + # empty string resolves to namespace + if not name: + return self._node.get_namespace() + # Check for global name: /foo/name resolves to /foo/name + if name[0] == sep: + return name + # Check for private name: ~name resolves to /caller_id/name + elif name[0] == priv_name: + if script_name[-1] == sep: + return script_name + name[1:] + return script_name + sep + name[1:] + return self._node.get_namespace() + name + def _parse_args(self, argv): parser = argparse.ArgumentParser(prog='rqt_plot', add_help=False) Plot.add_arguments(parser) @@ -84,7 +112,7 @@ def _parse_args(self, argv): base = sub_t[:sub_t.find(':')] # the first prefix includes a field name, so save then strip it off c_topics.append(base) - if not '/' in base: + if '/' not in base: parser.error("%s must contain a topic and field name" % sub_t) base = base[:base.rfind('/')] @@ -94,12 +122,11 @@ def _parse_args(self, argv): else: c_topics.append(sub_t) # 1053: resolve command-line topic names - import rosgraph - c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n) for n in c_topics] - if type(c_topics) == list: - topic_list.extend(c_topics) - else: - topic_list.append(c_topics) + topics, topic_types = self._node.get_topic_names_and_types() + + c_topics = [_resolve_topic_name('rqt_plot', n) for n in c_topics] + topic_list.extend(c_topics) + args.topics = topic_list return args diff --git a/src/rqt_plot/plot_widget.py b/src/rqt_plot/plot_widget.py index 52d0042..54351d0 100644 --- a/src/rqt_plot/plot_widget.py +++ b/src/rqt_plot/plot_widget.py @@ -31,47 +31,87 @@ # POSSIBILITY OF SUCH DAMAGE. import os -import rospkg -import roslib +import time +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 -import rospy - from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common import topic_helpers - -from . rosplot import ROSData, RosPlotException +from rqt_py_common import topic_helpers, message_helpers + +from rqt_plot.rosplot import ROSData, RosPlotException + + +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 + + end_array_idx = topic_type_str.find(']', start=array_idx + 1) + if end_array_idx < 0: + return None, False, None + + 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 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: + 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 -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) + 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:] - slot_type, is_array, array_size = roslib.msgs.parse_type(topic_type) - field_class = roslib.message.get_message_class(slot_type) + 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) + 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 - try: - field, _, field_index = roslib.msgs.parse_type(field) - except roslib.msgs.MsgSpecException: + field, _, field_index = _parse_type(field) + if field is None: message = "invalid field %s in topic %s" % (field, real_topic) return [], message - if field not in getattr(field_class, '__slots__', []): + 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_class._slot_types[field_class.__slots__.index(field)] - slot_type, slot_is_array, array_size = roslib.msgs.parse_type(slot_type) + 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 field_class = topic_helpers.get_type_class(slot_type) @@ -89,11 +129,10 @@ def get_plot_fields(topic_name): message = "topic %s is %s" % (topic_name, topic_kind) return [topic_name], message else: - if not roslib.msgs.is_valid_constant_type(slot_type): + if not topic_helpers.is_primitive_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) + 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) @@ -108,22 +147,23 @@ def get_plot_fields(topic_name): return [], message -def is_plottable(topic_name): - fields, message = get_plot_fields(topic_name) +def is_plottable(node, topic_name): + fields, message = get_plot_fields(node, topic_name) return len(fields) > 0, message class PlotWidget(QWidget): _redraw_interval = 40 - def __init__(self, initial_topics=None, start_paused=False): + 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 - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui') + _, 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')) @@ -136,10 +176,10 @@ def __init__(self, initial_topics=None, start_paused=False): self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) - self._topic_completer.update_topics() + self._topic_completer.update_topics(node) self.topic_edit.setCompleter(self._topic_completer) - self._start_time = rospy.get_time() + self._start_time = time.time() self._rosdata = {} self._remove_topic_menu = QMenu() @@ -192,7 +232,7 @@ def dragEnterEvent(self, event): topic_name = str(event.mimeData().text()) # check for plottable field type - plottable, message = is_plottable(topic_name) + plottable, message = is_plottable(self._node, topic_name) if plottable: event.acceptProposedAction() else: @@ -211,9 +251,9 @@ def dropEvent(self, event): def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): - self._topic_completer.update_topics() + self._topic_completer.update_topics(self._node) - plottable, message = is_plottable(topic_name) + plottable, message = is_plottable(self._node, topic_name) self.subscribe_topic_button.setEnabled(plottable) self.subscribe_topic_button.setToolTip(message) @@ -280,11 +320,11 @@ def make_remove_topic_function(x): def add_topic(self, topic_name): topics_changed = False - for topic_name in get_plot_fields(topic_name)[0]: + 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(topic_name, self._start_time) + 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] diff --git a/src/rqt_plot/rosplot.py b/src/rqt_plot/rosplot.py index b9995fb..a852647 100644 --- a/src/rqt_plot/rosplot.py +++ b/src/rqt_plot/rosplot.py @@ -38,18 +38,16 @@ import threading import time -import rosgraph -import roslib.message -import roslib.names -import rospy +from rqt_py_common.message_helpers import get_message_class from std_msgs.msg import Bool +from python_qt_binding.QtCore import qWarning class RosPlotException(Exception): pass -def _get_topic_type(topic): +def _get_topic_type(node, topic): """ subroutine for getting the topic type (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn) @@ -57,31 +55,26 @@ def _get_topic_type(topic): :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`` """ - try: - master = rosgraph.Master('/rosplot') - val = master.getTopicTypes() - except: - raise RosPlotException("unable to get list of topics from master") - matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')] - if matches: - t, t_type = matches[0] - if t_type == roslib.names.ANYTYPE: - return None, None, None - if t_type == topic: - return t_type, None - return t_type, t, topic[len(t):] - else: - return None, None, None - - -def get_topic_type(topic): + 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 + + +def get_topic_type(node, topic): """ 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`` """ - topic_type, real_topic, rest = _get_topic_type(topic) + topic_type, real_topic, rest = _get_topic_type(node, topic) if topic_type: return topic_type, real_topic, rest else: @@ -94,25 +87,26 @@ class ROSData(object): Subscriber to ROS topic that buffers incoming data """ - def __init__(self, topic, start_time): + def __init__(self, node, topic, start_time): self.name = topic self.start_time = start_time self.error = None + self.node = node self.lock = threading.Lock() self.buff_x = [] self.buff_y = [] - topic_type, real_topic, fields = get_topic_type(topic) + 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 = roslib.message.get_message_class(topic_type) - self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb) + data_class = get_message_class(topic_type) + self.sub = node.create_subscription(data_class, real_topic, self._ros_cb) else: self.error = RosPlotException("Can not resolve topic type of %s" % topic) def close(self): - self.sub.unregister() + self.node.destroy_subscription(self.sub) def _ros_cb(self, msg): """ @@ -124,10 +118,11 @@ def _ros_cb(self, msg): try: self.buff_y.append(self._get_data(msg)) # 944: use message header time if present - if msg.__class__._has_header: - self.buff_x.append(msg.header.stamp.to_sec() - self.start_time) + if hasattr(msg, 'header'): + stamped_time = msg.header.stamp.sec + msg.header.stamp.nanosec * 10**-9 + self.buff_x.append(stamped_time - self.start_time) else: - self.buff_x.append(rospy.get_time() - self.start_time) + self.buff_x.append(time.time() - self.start_time) # self.axes[index].plot(datax, buff_y) except AttributeError as e: self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e)))