diff --git a/ros2doctor/package.xml b/ros2doctor/package.xml index 3c70e1d8d..218641a5e 100644 --- a/ros2doctor/package.xml +++ b/ros2doctor/package.xml @@ -13,12 +13,15 @@ python3-catkin-pkg-modules python3-ifcfg python3-rosdistro-modules + rclpy ament_copyright ament_flake8 ament_pep257 ament_xmllint python3-pytest + ros_testing + std_msgs ament_python diff --git a/ros2doctor/ros2doctor/command/doctor.py b/ros2doctor/ros2doctor/command/doctor.py index 6d8df274b..599bab1b1 100644 --- a/ros2doctor/ros2doctor/command/doctor.py +++ b/ros2doctor/ros2doctor/command/doctor.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ros2cli.command import add_subparsers_on_demand from ros2cli.command import CommandExtension from ros2doctor.api import generate_reports from ros2doctor.api import run_checks @@ -35,9 +36,16 @@ def add_arguments(self, parser, cli_name): '--include-warnings', '-iw', action='store_true', help='Include warnings as failed checks. Warnings are ignored by default.' ) + # add arguments and sub-commands of verbs + add_subparsers_on_demand( + parser, cli_name, '_verb', 'ros2doctor.verb', required=False) def main(self, *, parser, args): """Run checks and print report to terminal based on user input args.""" + if hasattr(args, '_verb'): + extension = getattr(args, '_verb') + return extension.main(args=args) + # `ros2 doctor -r` if args.report: all_reports = generate_reports() diff --git a/ros2doctor/ros2doctor/verb/__init__.py b/ros2doctor/ros2doctor/verb/__init__.py new file mode 100644 index 000000000..2bbb6242d --- /dev/null +++ b/ros2doctor/ros2doctor/verb/__init__.py @@ -0,0 +1,44 @@ +# Copyright 2020 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 ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION +from ros2cli.plugin_system import satisfies_version + + +class VerbExtension: + """ + The extension point for 'doctor' verb extensions. + + The following properties must be defined: + * `NAME` (will be set to the entry point name) + + The following methods must be defined: + * `main` + + The following methods can be defined: + * `add_arguments` + """ + + NAME = None + EXTENSION_POINT_VERSION = '0.1' + + def __init__(self): + super(VerbExtension, self).__init__() + satisfies_version(PLUGIN_SYSTEM_VERSION, '^0.1') + + def add_arguments(self, parser, cli_name): + pass + + def main(self, *, args): + raise NotImplementedError() diff --git a/ros2doctor/ros2doctor/verb/hello.py b/ros2doctor/ros2doctor/verb/hello.py new file mode 100644 index 000000000..08a1b6ce2 --- /dev/null +++ b/ros2doctor/ros2doctor/verb/hello.py @@ -0,0 +1,249 @@ +# Copyright 2019 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 argparse import ArgumentTypeError +import os +import socket +import struct +import threading +import time + +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from ros2doctor.verb import VerbExtension + +from std_msgs.msg import String + +DEFAULT_GROUP = '225.0.0.1' +DEFAULT_PORT = 49150 + + +def positive_int(string: str) -> int: + try: + value = int(string) + except ValueError: + value = -1 + if value <= 0: + raise ArgumentTypeError('value must be a positive integer') + return value + + +class HelloVerb(VerbExtension): + """ + Check network connectivity between multiple hosts. + + This command can be invoked on multiple hosts to confirm that they can talk to each other + by using talker/listener, multicast send/receive to check topic discovering and + UDP communication. + This command outputs a summary table of msgs statistics at a custom period(s). + """ + + def add_arguments(self, parser, cli_name): + parser.add_argument( + '-t', '--topic', nargs='?', default='/canyouhearme', + help="Name of ROS topic to publish to (default: '/canyouhearme')") + parser.add_argument( + '-ep', '--emit-period', metavar='N', type=float, default=0.1, + help='Time period to publish/send one message (default: 0.1s)') + parser.add_argument( + '-pp', '--print-period', metavar='N', type=float, default=1.0, + help='Time period to print summary table (default: 1.0s)') + parser.add_argument( + '--ttl', type=positive_int, + help='TTL for multicast send (default: None)') + parser.add_argument( + '-1', '--once', action='store_true', default=False, + help='Publish and multicast send for one emit period then exit; used in test case.') + + def main(self, *, args): + global summary_table + summary_table = SummaryTable() + rclpy.init() + executor = SingleThreadedExecutor() + pub_node = Talker(args.topic, args.emit_period) + sub_node = Listener(args.topic) + executor.add_node(pub_node) + executor.add_node(sub_node) + try: + prev_time = time.time() + # pub/sub thread + exec_thread = threading.Thread(target=executor.spin) + exec_thread.start() + while True: + if (time.time() - prev_time > args.print_period): + summary_table.format_print_summary(args.topic, args.print_period) + summary_table.reset() + prev_time = time.time() + # multicast threads + send_thread = threading.Thread(target=_send, kwargs={'ttl': args.ttl}) + send_thread.daemon = True + receive_thread = threading.Thread(target=_receive) + receive_thread.daemon = True + receive_thread.start() + send_thread.start() + time.sleep(args.emit_period) + if args.once: + return summary_table + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + rclpy.shutdown() + pub_node.destroy_node() + sub_node.destroy_node() + + +class Talker(Node): + """Initialize talker node.""" + + def __init__(self, topic, time_period, *, qos=10): + node_name = socket.gethostname() + str(os.getpid()) + '_talker' + super().__init__(node_name) + self._i = 0 + self._pub = self.create_publisher(String, topic, qos) + self._timer = self.create_timer(time_period, self.timer_callback) + + def timer_callback(self): + msg = String() + hostname = socket.gethostname() + msg.data = f"hello, it's me {hostname}" + summary_table.increment_pub() + self._pub.publish(msg) + self._i += 1 + + +class Listener(Node): + """Initialize listener node.""" + + def __init__(self, topic, *, qos=10): + node_name = socket.gethostname() + str(os.getpid()) + '_listener' + super().__init__(node_name) + self._sub = self.create_subscription( + String, + topic, + self.sub_callback, + qos) + + def sub_callback(self, msg): + msg_data = msg.data.split() + pub_hostname = msg_data[-1] + if pub_hostname != socket.gethostname(): + summary_table.increment_sub(pub_hostname) + + +def _send(*, group=DEFAULT_GROUP, port=DEFAULT_PORT, ttl=None): + """Multicast send one message.""" + hostname = socket.gethostname() + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + if ttl is not None: + packed_ttl = struct.pack('b', ttl) + s.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, packed_ttl) + try: + s.sendto(f"hello, it's me {hostname}".encode('utf-8'), (group, port)) + summary_table.increment_send() + finally: + s.close() + + +def _receive(*, group=DEFAULT_GROUP, port=DEFAULT_PORT, timeout=None): + """Multicast receive.""" + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + try: + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + try: + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except AttributeError: + # not available on Windows + pass + s.bind(('', port)) + + s.settimeout(timeout) + + mreq = struct.pack('4sl', socket.inet_aton(group), socket.INADDR_ANY) + s.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) + try: + data, _ = s.recvfrom(4096) + data = data.decode('utf-8') + sender_hostname = data.split()[-1] + if sender_hostname != socket.gethostname(): + summary_table.increment_receive(sender_hostname) + finally: + s.setsockopt(socket.IPPROTO_IP, socket.IP_DROP_MEMBERSHIP, mreq) + finally: + s.close() + + +class SummaryTable(): + """Summarize number of msgs published/sent and subscribed/received.""" + + def __init__(self): + """Initialize empty summary table.""" + self.lock = threading.Lock() + self._pub = 0 + self._send = 0 + self._sub = {} + self._receive = {} + + def reset(self): + """Reset summary table to empty each time after printing.""" + with self.lock: + self._pub = 0 + self._send = 0 + self._sub = {} + self._receive = {} + + def increment_pub(self): + """Increment published msg count.""" + with self.lock: + self._pub += 1 + + def increment_sub(self, hostname): + """Increment subscribed msg count from different host(s).""" + with self.lock: + if hostname not in self._sub: + self._sub[hostname] = 1 + else: + self._sub[hostname] += 1 + + def increment_send(self): + """Increment multicast-sent msg count.""" + with self.lock: + self._send += 1 + + def increment_receive(self, hostname): + """Increment multicast-received msg count from different host(s).""" + with self.lock: + if hostname not in self._receive: + self._receive[hostname] = 1 + else: + self._receive[hostname] += 1 + + def format_print_summary(self, topic, print_period, *, group=DEFAULT_GROUP, port=DEFAULT_PORT): + """Print content in a table format.""" + def _format_print_summary_helper(table): + print('{:<15} {:<20} {:<10}'.format('', 'Hostname', f'Msg Count /{print_period}s')) + for name, count in table.items(): + print('{:<15} {:<20} {:<10}'.format('', name, count)) + + print('MULTIMACHINE COMMUNICATION SUMMARY') + print(f'Topic: {topic}, Published Msg Count: {self._pub}') + print('Subscribed from:') + _format_print_summary_helper(self._sub) + print( + f'Multicast Group/Port: {group}/{port}, ' + f'Sent Msg Count: {self._send}') + print('Received from:') + _format_print_summary_helper(self._receive) + print('-'*60) diff --git a/ros2doctor/setup.py b/ros2doctor/setup.py index 4067331d2..5b5bb90f5 100644 --- a/ros2doctor/setup.py +++ b/ros2doctor/setup.py @@ -51,5 +51,8 @@ 'TopicReport = ros2doctor.api.topic:TopicReport', 'PackageReport = ros2doctor.api.package:PackageReport', ], + 'ros2doctor.verb': [ + 'hello = ros2doctor.verb.hello:HelloVerb' + ] } ) diff --git a/ros2doctor/test/test_hello.py b/ros2doctor/test/test_hello.py new file mode 100644 index 000000000..8de0b3580 --- /dev/null +++ b/ros2doctor/test/test_hello.py @@ -0,0 +1,73 @@ +# Copyright 2020 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 argparse import Namespace + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing.actions +import launch_testing.markers + +import pytest + +from ros2doctor.verb.hello import HelloVerb +from ros2doctor.verb.hello import SummaryTable + + +@pytest.mark.rostest +@launch_testing.markers.keep_alive +def generate_test_description(): + return LaunchDescription([ + # Always restart daemon to isolate tests. + ExecuteProcess( + cmd=['ros2', 'daemon', 'stop'], + name='daemon-stop', + on_exit=[ + ExecuteProcess( + cmd=['ros2', 'daemon', 'start'], + name='daemon-start', + on_exit=[ + launch_testing.actions.ReadyToTest() + ] + ) + ] + ) + ]) + + +def _generate_expected_summary_table(): + """Generate expected summary table for one emit period on a single host.""" + expected_summary = SummaryTable() + # 1 pub/send per default emit period + expected_summary.increment_pub() + expected_summary.increment_send() + return expected_summary + + +def test_hello_single_host(): + """Run HelloVerb for one emit period on a single host.""" + args = Namespace() + args.topic = '/canyouhearme' + args.emit_period = 0.1 + args.print_period = 1.0 + args.ttl = None + args.once = True + hello_verb = HelloVerb() + summary = hello_verb.main(args=args) + expected_summary = _generate_expected_summary_table() + assert summary._pub == expected_summary._pub + assert summary._sub == expected_summary._sub + assert summary._send == expected_summary._send + assert summary._receive == expected_summary._receive diff --git a/ros2doctor/test/test_topic.py b/ros2doctor/test/test_topic.py index c8650ad7b..19edf89c6 100644 --- a/ros2doctor/test/test_topic.py +++ b/ros2doctor/test/test_topic.py @@ -12,11 +12,40 @@ # See the License for the specific language governing permissions and # limitations under the License. +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing.actions +import launch_testing.markers + +import pytest + from ros2doctor.api import Report from ros2doctor.api.topic import TopicCheck from ros2doctor.api.topic import TopicReport +@pytest.mark.rostest +@launch_testing.markers.keep_alive +def generate_test_description(): + return LaunchDescription([ + # Always restart daemon to isolate tests. + ExecuteProcess( + cmd=['ros2', 'daemon', 'stop'], + name='daemon-stop', + on_exit=[ + ExecuteProcess( + cmd=['ros2', 'daemon', 'start'], + name='daemon-start', + on_exit=[ + launch_testing.actions.ReadyToTest() + ] + ) + ] + ) + ]) + + def test_topic_check(): """Assume no topics are publishing or subscribing other than whitelisted ones.""" topic_check = TopicCheck()