diff --git a/test_discovery/CMakeLists.txt b/test_discovery/CMakeLists.txt new file mode 100644 index 00000000..c4a027c3 --- /dev/null +++ b/test_discovery/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(test_discovery) + +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(test_msgs REQUIRED) + +add_executable(publish_once src/publish_once.cpp) +target_compile_features(publish_once PUBLIC cxx_std_17) +target_link_libraries(publish_once PRIVATE + rclcpp::rclcpp + ${test_msgs_TARGETS} +) + +add_executable(subscribe_once src/subscribe_once.cpp) +target_compile_features(subscribe_once PUBLIC cxx_std_17) +target_link_libraries(subscribe_once PRIVATE + rclcpp::rclcpp + ${test_msgs_TARGETS} +) + +install(TARGETS publish_once subscribe_once + DESTINATION lib/${PROJECT_NAME}) + +install(FILES + roottests/test_discovery.py + roottests/conftest.py + DESTINATION share/${PROJECT_NAME}/roottests/) + +install(PROGRAMS + scripts/run_root_tests.py + DESTINATION lib/${PROJECT_NAME}/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_pytest_test(test_discovery tests/test_discovery.py TIMEOUT 1600) +endif() + +ament_package() diff --git a/test_discovery/README.md b/test_discovery/README.md new file mode 100644 index 00000000..fd580efc --- /dev/null +++ b/test_discovery/README.md @@ -0,0 +1,40 @@ +# test_discovery + +This package tests the use of `ROS_AUTOMATIC_DISCOVERY_RANGE` and `ROS_STATIC_PEERS` environment variables. +There are two sets of tests: automated tests that run when testing this package, and semiautomated tests that must be run manually after building this package. + +# Automated tests + +The automated tests run when testing this package. +They test only the cases that apply when two processes are on the same host. + +## Semi-automated tests + +The semiautomated tests use `mininet` to test discovery behavior across two different (virtual) hosts. +These tests require `root` access, and a working `mininet` install. + +### Installing prerequisites + +A working `mininet` install has only been tested on an Ubuntu based machine. +If you're running in a container, that container will need root priviledges. + +First install the necessary dependencies: + +```bash +sudo apt install \ + iputils-ping \ + iproute2 \ + mininet \ + openvswitch-switch \ + openvswitch-testcontroller +``` + +Next, make sure the openvswitch service is running + +```bash +sudo service openvswitch-switch start +``` + +You're now ready to run the tests. + +### Running the semi-automated tests diff --git a/test_discovery/package.xml b/test_discovery/package.xml new file mode 100644 index 00000000..fa5a07b1 --- /dev/null +++ b/test_discovery/package.xml @@ -0,0 +1,34 @@ + + + + test_discovery + 0.14.0 + + Test discovery behaviors in ROS 2 using semiautomated tests. + + + Brandon Ong + + Apache License 2.0 + + Shane Loretz + + ament_cmake_ros + + + rclcpp + test_msgs + pytest + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + test_msgs + + ros2cli + + + ament_cmake + + diff --git a/test_discovery/roottests/conftest.py b/test_discovery/roottests/conftest.py new file mode 100644 index 00000000..fa9ded54 --- /dev/null +++ b/test_discovery/roottests/conftest.py @@ -0,0 +1,22 @@ +# Copyright 2023 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. + +def pytest_addoption(parser): + parser.addoption('--ros-workspaces', action='store') + parser.addoption('--rmws', action='store') + + +def pytest_generate_tests(metafunc): + if 'rmw' in metafunc.fixturenames: + metafunc.parametrize('rmw', metafunc.config.option.rmws.split(':')) diff --git a/test_discovery/roottests/test_discovery.py b/test_discovery/roottests/test_discovery.py new file mode 100644 index 00000000..377d10bb --- /dev/null +++ b/test_discovery/roottests/test_discovery.py @@ -0,0 +1,123 @@ +# Copyright 2023 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. + +# Run discovery tests +# 1. Install dependencies +# sudo apt install iputils-ping iproute2 mininet openvswitch-switch openvswitch-testcontroller +# 2. Start openvswitch service if not already running +# sudo service openvswitch-switch start + +from mininet.net import Mininet +from mininet.topo import MinimalTopo +import pytest + + +RANGES = [ + # None, # idential to LOCALHOST, but takes too long to test here + 'OFF', + 'SUBNET', + 'LOCALHOST', +] + + +class MininetFixture: + __slots__ = ( + 'net', + 'h1', + 'h2', + ) + + +def h1_ipv4(net: MininetFixture) -> str: + return net.h1.IP() + + +def h2_ipv4(net: MininetFixture) -> str: + return net.h2.IP() + + +def no_peer(net: MininetFixture) -> str: + return '' + + +@pytest.fixture() +def mn(): + f = MininetFixture() + f.net = Mininet(topo=MinimalTopo()) + f.h1 = f.net.getNodeByName('h1') + f.h2 = f.net.getNodeByName('h2') + + f.net.start() + yield f + f.net.stop() + + +# TODO(sloretz) figure out ROS workspace path from environment variables +@pytest.fixture(scope='session') +def ros_ws(pytestconfig): + return pytestconfig.getoption('ros_workspaces').split(':') + + +def make_env_str(ros_ws, rmw, discovery_range, peer): + cmd = [] + for ws in ros_ws: + cmd.extend([ + '.', + f'"{ws}/setup.bash"', + '&&', + ]) + cmd.extend([ + f'RMW_IMPLEMENTATION={rmw}', + f'ROS_STATIC_PEERS="{peer}"', + ]) + if discovery_range is not None: + cmd.append(f'ROS_AUTOMATIC_DISCOVERY_RANGE={discovery_range}') + return ' '.join(cmd) + + +@pytest.mark.parametrize('sub_peer', (no_peer, h1_ipv4)) +@pytest.mark.parametrize('sub_range', RANGES) +@pytest.mark.parametrize('pub_peer', (no_peer, h2_ipv4)) +@pytest.mark.parametrize('pub_range', RANGES) +def test_differenthost(mn, ros_ws, rmw, pub_range, pub_peer, sub_range, sub_peer): + pub_peer = pub_peer(mn) + sub_peer = sub_peer(mn) + + pub_env = make_env_str(ros_ws, rmw, pub_range, pub_peer) + sub_env = make_env_str(ros_ws, rmw, sub_range, sub_peer) + pub_cmd = pub_env + ' ros2 run test_discovery publish_once > /dev/null &' + sub_cmd = sub_env + ' ros2 run test_discovery subscribe_once' + print('$', pub_cmd) + print('$', sub_cmd) + + mn.h1.cmd(pub_cmd) + result = mn.h2.cmd(sub_cmd) + + # Invalid node configuration could make OFF tests appear to succeed + assert 'test_discovery: node successfully created' in result.strip() + + message_received = 'test_discovery: message was received' in result.strip() + + if 'OFF' in (pub_range, sub_range): + # If either has discovery off then it won't succeed + assert not message_received, result.strip() + elif pub_peer or sub_peer: + # if either has a static peer set, discovery should succeed + assert message_received, result.strip() + elif 'SUBNET' == pub_range and 'SUBNET' == sub_range: + # With no static peer, succeed only if both are set to SUBNET + assert message_received, result.strip() + else: + # All other cases don't discover each other + assert not message_received, result.strip() diff --git a/test_discovery/scripts/run_root_tests.py b/test_discovery/scripts/run_root_tests.py new file mode 100644 index 00000000..58a7d0ee --- /dev/null +++ b/test_discovery/scripts/run_root_tests.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import argparse +import os +import sys + +from ament_index_python import get_package_share_path +from ament_index_python import get_resources + + +def get_rmw_implementations(): + resources = list(get_resources('rmw_typesupport').keys()) + if 'rmw_implementation' in resources: + resources.remove('rmw_implementation') + return tuple(resources) + + +def get_tests_dir(): + pkg_path = get_package_share_path('test_discovery') + return pkg_path / 'roottests' + + +def get_workspaces(): + # Get an ordered list of workspaces that are sourced + prefixes = os.environ['AMENT_PREFIX_PATH'] + if not prefixes: + raise ValueError('No ROS/Colcon workspace sourced') + workspaces = set() + for prefix in prefixes.split(':'): + if not prefix: + # env var might have began or ended with a ':' + continue + # If there exists a parent folder containing a setup.bash + # then assume this is an isolated colcon workspace + if os.path.exists(os.path.join(prefix, '../setup.bash')): + workspaces.add(os.path.dirname(prefix)) + else: + # Assume a merged ament/colcon workspace + workspaces.add(prefix) + return tuple(workspaces) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--rmw', default=None) + parser.add_argument('--select', default=None) + args = parser.parse_args() + + rmw_implementations = get_rmw_implementations() + if args.rmw: + if args.rmw not in rmw_implementations: + raise ValueError(f'{args.rmw} is not an installed rmw: {rmw_implementations}') + rmw_implementations = [args.rmw] + + cmd = [ + 'sudo', + sys.executable, + '-m', + 'pytest', + '-c', + str(get_tests_dir() / 'conftest.py'), + ] + if args.select: + cmd.extend([ + '-k', + args.select, + ]) + cmd.extend([ + f'--rmws={":".join(rmw_implementations)}', + f'--ros-workspaces={":".join(get_workspaces())}', + str(get_tests_dir() / 'test_discovery.py'), + ]) + + print('Executing the following command:') + print('================================') + print('$', *cmd) + print('================================') + + os.execvp(cmd[0], cmd) + + +if __name__ == '__main__': + main() diff --git a/test_discovery/src/publish_once.cpp b/test_discovery/src/publish_once.cpp new file mode 100644 index 00000000..a58fabf0 --- /dev/null +++ b/test_discovery/src/publish_once.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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. + +#include +#include + +#include +#include + + +constexpr double kMaxDiscoveryTime = 10; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("publish_once"); + auto publisher = node->create_publisher("test_topic", 10); + + auto clock = node->get_clock(); + + auto end_time = clock->now() + rclcpp::Duration::from_seconds(kMaxDiscoveryTime); + while (rclcpp::ok() && publisher->get_subscription_count() == 0) { + if (clock->now() >= end_time) { + return 0; + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + publisher->publish(test_msgs::msg::Builtins()); + + // Do nothing until killed. + rclcpp::spin(node); + return 0; +} diff --git a/test_discovery/src/subscribe_once.cpp b/test_discovery/src/subscribe_once.cpp new file mode 100644 index 00000000..dafe7a2b --- /dev/null +++ b/test_discovery/src/subscribe_once.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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. + +#include +#include + +#include +#include + +constexpr double kTimeout = 10; + +void topic_callback(const test_msgs::msg::Builtins &) +{ + std::cout << "test_discovery: message was received\n" << std::flush; + std::exit(0); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("subscribe_once"); + auto subscription = + node->create_subscription("test_topic", 10, topic_callback); + + std::cout << "test_discovery: node successfully created\n" << std::flush; + + auto clock = node->get_clock(); + auto end_time = clock->now() + rclcpp::Duration::from_seconds(kTimeout); + while (rclcpp::ok() && clock->now() <= end_time) { + rclcpp::spin_some(node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + return 0; +} diff --git a/test_discovery/tests/test_discovery.py b/test_discovery/tests/test_discovery.py new file mode 100644 index 00000000..a698e021 --- /dev/null +++ b/test_discovery/tests/test_discovery.py @@ -0,0 +1,123 @@ +# Copyright 2023 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. + +import os +import pathlib +import subprocess +import sys + +from ament_index_python import get_package_prefix +from ament_index_python import get_resources +import pytest + + +RANGES = [ + # None, # idential to LOCALHOST, but takes too long to test here + 'OFF', + 'SUBNET', + 'LOCALHOST', +] + + +PEERS = [ + None, + '127.0.0.1', +] + + +def get_rmw_implementations(): + resources = list(get_resources('rmw_typesupport').keys()) + if 'rmw_implementation' in resources: + resources.remove('rmw_implementation') + return tuple(resources) + + +def get_executable(name): + p = pathlib.Path(get_package_prefix('test_discovery')) + return str(p / 'lib' / 'test_discovery' / name) + + +def make_env(rmw, discovery_range, peer): + env = dict(os.environ) + env['RMW_IMPLEMENTATION'] = rmw + if discovery_range is None: + del env['ROS_AUTOMATIC_DISCOVERY_RANGE'] + else: + env['ROS_AUTOMATIC_DISCOVERY_RANGE'] = discovery_range + if peer is None: + peer = '' + env['ROS_STATIC_PEERS'] = peer + return env + + +def communicate(name, proc): + try: + stdout, stderr = proc.communicate(timeout=10) + except subprocess.TimeoutExpired: + proc.kill() + stdout, stderr = proc.communicate() + stdout = stdout.decode() + stderr = stderr.decode() + for line in stdout.split('\n'): + sys.stdout.write(f'{name}[stdout]: {line}\n') + for line in stderr.split('\n'): + sys.stderr.write(f'{name}[stderr]: {line}\n') + return stdout, stderr + + +@pytest.mark.parametrize('sub_peer', PEERS) +@pytest.mark.parametrize('sub_range', RANGES) +@pytest.mark.parametrize('pub_peer', PEERS) +@pytest.mark.parametrize('pub_range', RANGES) +@pytest.mark.parametrize('rmw', get_rmw_implementations()) +def test_thishost(rmw, pub_range, pub_peer, sub_range, sub_peer): + # For same host tests, setting a static peer while using SUBNET + # doesn't make a lot of sense. + # Further, it cause test failures with rmw_fastrtps_* + # When there's an initial peer to localhost, Fast-DDS will try to discover + # peers on localhost using unicast discovery, but that uses the + # default maxInitialPeersRange value of 4, which is a very small number of + # peers. + # Any other ROS nodes on a system (such as daemons, or test processes that + # weren't cleaned up) will cause this test to fail. + if 'SUBNET' == sub_range and sub_peer is not None: + pytest.skip('Skipping samehost SUBNET with static peer') + if 'SUBNET' == pub_range and pub_peer is not None: + pytest.skip('Skipping samehost SUBNET with static peer') + + pub_env = make_env(rmw, pub_range, pub_peer) + sub_env = make_env(rmw, sub_range, sub_peer) + pub_cmd = [get_executable('publish_once')] + sub_cmd = [get_executable('subscribe_once')] + + pub_proc = subprocess.Popen( + pub_cmd, env=pub_env, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + sub_proc = subprocess.Popen( + sub_cmd, env=sub_env, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + + stdout, _ = communicate('sub', sub_proc) + # Invalid node configuration could make OFF tests appear to succeed + assert 'test_discovery: node successfully created' in stdout + + message_received = 'test_discovery: message was received' in stdout + pub_proc.kill() + + communicate('pub', pub_proc) + + if 'OFF' in (pub_range, sub_range): + # If either has discovery off then it won't succeed + assert not message_received + else: + # All other cases discover each other + assert message_received