From 1208b81650c0e8bad5ad4db300f93ef65ecc027f Mon Sep 17 00:00:00 2001 From: dhood Date: Wed, 28 Jun 2017 22:18:56 -0700 Subject: [PATCH] Update topic monitor for get_topic_names_and_types changes (#149) * Topics returned by get_topic_names_and_types have leading forward slashes now * get_topic_names_and_types returns a list of types now * Fix a bug in the reception of best effort messages * Kinder failing without matplotlib * Add missing __init__.py files * Print warning when topics which appropriate names are ignored because of types * No issues with empty waitsets anymore (There's always a signal handler) * Add linters * Install the whole package rather than using py_modules * Don't publish on topic with the same name token The different types cause connext to get mixed up, see https://github.com/ros2/rmw_connext/issues/234 * Flake8 import order wants different things on different machines? * Add launch as exec_depend --- topic_monitor/package.xml | 4 ++ topic_monitor/setup.py | 11 +--- topic_monitor/test/test_flake8.py | 20 +++++++ topic_monitor/test/test_pep257.py | 20 +++++++ topic_monitor/topic_monitor/__init__.py | 0 .../topic_monitor/launch_files/__init__.py | 0 .../topic_monitor/scripts/__init__.py | 0 .../topic_monitor/scripts/data_publisher.py | 1 + .../scripts/ros1/data_pub_ros1.py | 1 + .../topic_monitor/scripts/topic_monitor.py | 56 ++++++++++++++----- 10 files changed, 90 insertions(+), 23 deletions(-) create mode 100644 topic_monitor/test/test_flake8.py create mode 100644 topic_monitor/test/test_pep257.py create mode 100644 topic_monitor/topic_monitor/__init__.py create mode 100644 topic_monitor/topic_monitor/launch_files/__init__.py create mode 100644 topic_monitor/topic_monitor/scripts/__init__.py diff --git a/topic_monitor/package.xml b/topic_monitor/package.xml index b570a71fe..a79c02c17 100644 --- a/topic_monitor/package.xml +++ b/topic_monitor/package.xml @@ -10,10 +10,14 @@ ament_python rclpy + launch rclpy ros2run std_msgs + ament_flake8 + ament_pep257 + ament_python diff --git a/topic_monitor/setup.py b/topic_monitor/setup.py index 3e655ad61..3adc346fd 100644 --- a/topic_monitor/setup.py +++ b/topic_monitor/setup.py @@ -1,5 +1,6 @@ from ament_python.data_files import get_data_files from ament_python.script_dir import install_scripts_to_libexec +from setuptools import find_packages from setuptools import setup package_name = 'topic_monitor' @@ -9,14 +10,7 @@ setup( name='topic_monitor', version='0.0.0', - packages=[], - py_modules=[ - 'topic_monitor.launch_files.launch_depth_demo', - 'topic_monitor.launch_files.launch_fragmentation_demo', - 'topic_monitor.launch_files.launch_reliability_demo', - 'topic_monitor.scripts.data_publisher', - 'topic_monitor.scripts.topic_monitor', - ], + packages=find_packages(exclude=['test']), data_files=data_files, install_requires=[ 'launch', @@ -31,6 +25,7 @@ ], description='Package containing tools for monitoring ROS 2 topics.', license='Apache License, Version 2.0', + test_suite='test', entry_points={ 'console_scripts': [ 'data_publisher = topic_monitor.scripts.data_publisher:main', diff --git a/topic_monitor/test/test_flake8.py b/topic_monitor/test/test_flake8.py new file mode 100644 index 000000000..ca8aa2cd6 --- /dev/null +++ b/topic_monitor/test/test_flake8.py @@ -0,0 +1,20 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main + + +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/topic_monitor/test/test_pep257.py b/topic_monitor/test/test_pep257.py new file mode 100644 index 000000000..dd3ea28ee --- /dev/null +++ b/topic_monitor/test/test_pep257.py @@ -0,0 +1,20 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main + + +def test_pep257(): + rc = main(argv=[]) + assert rc == 0, 'Found code style errors / warnings' diff --git a/topic_monitor/topic_monitor/__init__.py b/topic_monitor/topic_monitor/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/topic_monitor/topic_monitor/launch_files/__init__.py b/topic_monitor/topic_monitor/launch_files/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/topic_monitor/topic_monitor/scripts/__init__.py b/topic_monitor/topic_monitor/scripts/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/topic_monitor/topic_monitor/scripts/data_publisher.py b/topic_monitor/topic_monitor/scripts/data_publisher.py index e974b02c8..12857e1db 100755 --- a/topic_monitor/topic_monitor/scripts/data_publisher.py +++ b/topic_monitor/topic_monitor/scripts/data_publisher.py @@ -122,5 +122,6 @@ def publish_msg(val): sleep(0.1) raise + if __name__ == '__main__': main() diff --git a/topic_monitor/topic_monitor/scripts/ros1/data_pub_ros1.py b/topic_monitor/topic_monitor/scripts/ros1/data_pub_ros1.py index 7781ffe3b..c84321290 100755 --- a/topic_monitor/topic_monitor/scripts/ros1/data_pub_ros1.py +++ b/topic_monitor/topic_monitor/scripts/ros1/data_pub_ros1.py @@ -64,5 +64,6 @@ def publish_msg(val): publish_msg(-1) raise + if __name__ == '__main__': main() diff --git a/topic_monitor/topic_monitor/scripts/topic_monitor.py b/topic_monitor/topic_monitor/scripts/topic_monitor.py index bb5521c4e..514ffb344 100755 --- a/topic_monitor/topic_monitor/scripts/topic_monitor.py +++ b/topic_monitor/topic_monitor/scripts/topic_monitor.py @@ -18,8 +18,8 @@ import time import rclpy -from rclpy.qos import QoSReliabilityPolicy from rclpy.qos import qos_profile_default +from rclpy.qos import QoSReliabilityPolicy from std_msgs.msg import Float32, Header @@ -55,7 +55,7 @@ def get_data_from_msg(self, msg): data = msg.frame_id idx = data.find('_') data = data[:idx] if idx != -1 else data - return int(data) + return int(data) if data else 0 def topic_data_callback(self, msg): received_value = self.get_data_from_msg(msg) @@ -109,7 +109,7 @@ class TopicMonitor: """Monitor of a set of topics that match a specified topic name pattern.""" def __init__(self, window_size): - self.data_topic_pattern = re.compile("((?P\w*)_data_?(?P\w*))") + self.data_topic_pattern = re.compile("(/(?P\w*)_data_?(?P\w*))") self.monitored_topics = {} self.monitored_topics_lock = Lock() self.publishers = {} @@ -129,6 +129,7 @@ def add_monitored_topic( topic_name, monitored_topic.topic_data_callback, qos_profile=qos_profile) + assert sub # prevent unused warning # Create a timer for maintaining the expected value received on the topic expected_value_timer = node.create_timer( @@ -142,8 +143,15 @@ def add_monitored_topic( allowed_latency_timer.cancel() # Create a publisher for the reception rate of the topic + reception_rate_topic_name = self.reception_rate_topic_name + topic_name + + # TODO(dhood): remove this workaround + # once https://github.com/ros2/rmw_connext/issues/234 is resolved + reception_rate_topic_name += "_" + + print('Publishing reception rate on topic: %s' % reception_rate_topic_name) reception_rate_publisher = node.create_publisher( - Float32, self.reception_rate_topic_name + '/' + topic_name) + Float32, reception_rate_topic_name) with self.monitored_topics_lock: monitored_topic.expected_value_timer = expected_value_timer @@ -301,21 +309,37 @@ def stop(self): def run_topic_listening(node, topic_monitor, options): """Subscribe to relevant topics and manage the data received from susbcriptions.""" + already_ignored_topics = set() while rclpy.ok(): # Check if there is a new topic online # TODO(dhood): use graph events rather than polling topic_names_and_types = node.get_topic_names_and_types() - for topic_name, type_name in topic_names_and_types: - if not topic_monitor.is_supported_type(type_name): - continue - + for topic_name, type_names in topic_names_and_types: # Infer the appropriate QoS profile from the topic name topic_info = topic_monitor.get_topic_info(topic_name) if topic_info is None: # The topic is not for being monitored continue + if len(type_names) != 1: + if topic_name not in already_ignored_topics: + print( + "Warning: ignoring topic '%s', which has more than one type: [%s]" + % (topic_name, ', '.join(type_names))) + already_ignored_topics.add(topic_name) + continue + + type_name = type_names[0] + if not topic_monitor.is_supported_type(type_name): + if topic_name not in already_ignored_topics: + print( + "Warning: ignoring topic '%s' because its message type (%s)" + "is not suported." + % (topic_name, type_name)) + already_ignored_topics.add(topic_name) + continue + is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics if is_new_topic: # Register new topic with the monitor @@ -323,15 +347,14 @@ def run_topic_listening(node, topic_monitor, options): qos_profile.depth = QOS_DEPTH if topic_info['reliability'] == 'best_effort': qos_profile.reliability = \ - QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE + QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT topic_monitor.add_monitored_topic( Header, topic_name, node, qos_profile, options.expected_period, options.allowed_latency, options.stale_time) - if topic_monitor.monitored_topics: - # Wait for messages with a timeout, otherwise this thread will block any other threads - # until a message is received - rclpy.spin_once(node, timeout_sec=0.05) + # Wait for messages with a timeout, otherwise this thread will block any other threads + # until a message is received + rclpy.spin_once(node, timeout_sec=0.05) def main(): @@ -362,8 +385,11 @@ def main(): args = parser.parse_args() if args.show_display: - global plt - import matplotlib.pyplot as plt + try: + global plt + import matplotlib.pyplot as plt + except: + raise RuntimeError('The --display option requires matplotlib to be installed') topic_monitor = TopicMonitor(args.window_size)