diff --git a/test/ros_test_runner.py b/test/ros_test_runner.py index 59e56c98543..45a2165169b 100755 --- a/test/ros_test_runner.py +++ b/test/ros_test_runner.py @@ -5,6 +5,7 @@ import json import os import psutil # type: ignore import signal +import socket import subprocess import sys import time @@ -62,6 +63,86 @@ class MicroXrceAgent: self.start_process() +class ZenohRouter: + """rmw_zenoh router: PX4's zenoh client and the rmw_zenoh_cpp test nodes meet here.""" + + PORT = 7447 + + def __init__(self, verbose: bool): + self._verbose = verbose + self._binary_name = 'rmw_zenohd' + self._proc = None + + def is_running(self) -> bool: + return is_running(self._binary_name) + + def start_process(self): + if self._verbose: + print('Starting zenoh router') + assert self._proc is None + + # Run the router binary directly: killing a 'ros2 run' wrapper would orphan it. + prefix = subprocess.run(['ros2', 'pkg', 'prefix', 'rmw_zenoh_cpp'], + capture_output=True, text=True) + if prefix.returncode != 0: + raise RuntimeError('rmw_zenoh_cpp not found. Install ros-$ROS_DISTRO-rmw-zenoh-cpp ' + 'or run the router manually before running this script') + binary = os.path.join(prefix.stdout.strip(), 'lib', 'rmw_zenoh_cpp', self._binary_name) + self._proc = subprocess.Popen([binary]) + self._wait_until_listening() + + def _wait_until_listening(self, timeout_s: float = 10.0): + """PX4 opens its zenoh session once at startup, so the router must accept connections first.""" + assert self._proc is not None + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if self._proc.poll() is not None: + raise RuntimeError(f'Zenoh router exited with code {self._proc.returncode}') + try: + with socket.create_connection(('127.0.0.1', self.PORT), timeout=0.2): + return + except OSError: + time.sleep(0.1) + raise RuntimeError(f'Zenoh router did not listen on port {self.PORT} within {timeout_s}s') + + def stop_process_if_started(self): + if self._proc is None: + return + + if self._verbose: + print('Stopping zenoh router') + self._proc.kill() + self._proc.wait() + self._proc = None + + def restart(self): + """Force a fresh router so liveliness tokens of the previous PX4 instance do not leak. + + On POSIX every PX4 instance announces the same node GUID, so a stale token would + make the next test discover the FMU before the new instance is up. + """ + self.stop_process_if_started() + self.start_process() + + +MIDDLEWARES = ('uxrce_dds', 'zenoh') + + +def create_middleware_bridge(middleware: str, verbose: bool): + """The process PX4 and the ROS 2 nodes connect through: XRCE Agent or zenoh router.""" + if middleware == 'zenoh': + return ZenohRouter(verbose) + return MicroXrceAgent(verbose) + + +def middleware_environment(middleware: str) -> Dict[str, str]: + """Environment inherited by PX4 (see the posix rcS) and by the ROS 2 test binary.""" + env = {'PX4_ROS2_MIDDLEWARE': middleware} + if middleware == 'zenoh': + env['RMW_IMPLEMENTATION'] = 'rmw_zenoh_cpp' + return env + + class TesterInterfaceRos(test_runner.TesterInterface): def __init__(self, ros_package_build_dir: str): @@ -151,6 +232,9 @@ def create_argument_parser() -> argparse.ArgumentParser: default=None, help="path which contains the integration_tests binary. " "If not provided, it is determined via 'ros2 pkg'") + parser.add_argument("--middleware", choices=MIDDLEWARES, default='uxrce_dds', + help="ROS 2 middleware between PX4 and the tests " + "(default: uxrce_dds)") return parser @@ -216,21 +300,24 @@ def main() -> NoReturn: parser.error("No test cases selected; check --config-file, --model and --case") signal.signal(signal.SIGINT, tester.sigint_handler) - # Automatically start & stop the XRCE Agent if not running already - micro_xrce_agent = MicroXrceAgent(args.verbose) - agent_managed_here = not micro_xrce_agent.is_running() - if agent_managed_here: - micro_xrce_agent.start_process() + # PX4 and the test binary inherit the environment through process_helper.Runner + os.environ.update(middleware_environment(args.middleware)) - def restart_agent(_model: str, _case: str) -> None: - micro_xrce_agent.restart() + # Automatically start & stop the XRCE Agent or zenoh router if not running already + bridge = create_middleware_bridge(args.middleware, args.verbose) + bridge_managed_here = not bridge.is_running() + if bridge_managed_here: + bridge.start_process() - tester.pre_test_hook = restart_agent + def restart_bridge(_model: str, _case: str) -> None: + bridge.restart() + + tester.pre_test_hook = restart_bridge try: result = tester.run() finally: - micro_xrce_agent.stop_process_if_started() + bridge.stop_process_if_started() sys.exit(0 if result else 1) diff --git a/test/ros_tests/config-sih-zenoh.json b/test/ros_tests/config-sih-zenoh.json new file mode 100644 index 00000000000..fc9096c1368 --- /dev/null +++ b/test/ros_tests/config-sih-zenoh.json @@ -0,0 +1,14 @@ +{ + "mode": "sitl", + "simulator": "sih", + "model_prefix": "sihsim_", + "tests": + [ + { + "model": "quadx", + "vehicle": "quadx", + "test_filter": "ModesTest.*", + "timeout_min": 10 + } + ] +} diff --git a/test/test_ros_test_runner.py b/test/test_ros_test_runner.py index 6e4981f2791..bab2c9f3b6f 100644 --- a/test/test_ros_test_runner.py +++ b/test/test_ros_test_runner.py @@ -44,6 +44,43 @@ class RosTestRunnerArgumentsTest(unittest.TestCase): self.assertEqual(args.build_dir, "/tmp/custom-sih") self.assertEqual(args.config_file, "test/ros_tests/config-sih.json") + def test_middleware_defaults_to_uxrce_dds(self): + args = self.parser.parse_args([]) + self.assertEqual(args.middleware, "uxrce_dds") + self.assertIsInstance( + ros_test_runner.create_middleware_bridge(args.middleware, False), + ros_test_runner.MicroXrceAgent, + ) + self.assertEqual( + ros_test_runner.middleware_environment(args.middleware), + {"PX4_ROS2_MIDDLEWARE": "uxrce_dds"}, + ) + + def test_zenoh_uses_router_and_rmw_zenoh(self): + args = self.parser.parse_args(["--middleware", "zenoh"]) + self.assertIsInstance( + ros_test_runner.create_middleware_bridge(args.middleware, False), + ros_test_runner.ZenohRouter, + ) + self.assertEqual( + ros_test_runner.middleware_environment(args.middleware), + {"PX4_ROS2_MIDDLEWARE": "zenoh", "RMW_IMPLEMENTATION": "rmw_zenoh_cpp"}, + ) + + def test_zenoh_config_runs_only_mode_tests(self): + # Zenoh has no ROS time sync yet, so EKF2 rejects the position interface inputs + config = json.loads( + (self.repo / "test/ros_tests/config-sih-zenoh.json").read_text() + ) + self.assertEqual(config["simulator"], "sih") + self.assertEqual( + [test["test_filter"] for test in config["tests"]], ["ModesTest.*"] + ) + + def test_unknown_middleware_is_rejected(self): + with self.assertRaises(SystemExit): + self.parser.parse_args(["--middleware", "fastdds"]) + if __name__ == "__main__": unittest.main()