#!/usr/bin/env python3 import argparse import json import os import psutil # type: ignore import signal import socket import subprocess import sys import time from mavsdk_tests.integration_test_runner import test_runner, process_helper as ph, logger_helper from typing import Any, Dict, List, NoReturn class MicroXrceAgent: def __init__(self, verbose: bool): self._verbose = verbose # Potential binary names to check for self._binary_names = ['MicroXRCEAgent', 'micro-xrce-dds-agent'] self._proc = None def is_running(self) -> bool: return any(is_running(name) for name in self._binary_names) def start_process(self): if self._verbose: print('Starting micro-xrce-dds-agent') assert self._proc is None for name in self._binary_names: try: self._proc = subprocess.Popen([name] + 'udp4 -p 8888'.split()) except FileNotFoundError: pass else: # Process was started break if self._proc is None: raise RuntimeError(f'Failed to start agent, tried these binaries: {self._binary_names}.\n' 'Make sure it is installed and available. ' 'You can also run it manually before running this script') def stop_process_if_started(self): if self._proc is None: return if self._verbose: print('Stopping micro-xrce-dds-agent') self._proc.kill() self._proc.wait() self._proc = None def restart(self): """Force a fresh Agent process so DDS graph state does not leak across tests. The Agent retains writer entries from prior PX4 instances; a fresh PX4 reconnects but the stale entries make count_publishers() return >0 before the new writers are matched, breaking waitForFMU's two-phase discovery in px4-ros2-interface-lib. """ self.stop_process_if_started() # Give the OS a moment to release the UDP port before rebinding. time.sleep(0.2) 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): self._ros_package_build_dir = ros_package_build_dir def query_test_cases(self, build_dir: str, filter: str) -> List[str]: cmd = os.path.join(self._ros_package_build_dir, 'integration_tests') args = ["--gtest_list_tests", "--gtest_filter=" + filter] # Example output: # Running main() from ros2_install/install/gtest_vendor/src/gtest_vendor/src/gtest_main.cc # Tester. # runModeTests p = subprocess.Popen( [cmd] + args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) assert p.stdout is not None lines = str(p.stdout.read().decode("utf-8")).strip().split('\n') if lines[0].startswith('Running main'): del lines[0] cases = [] test_suite_name = None for line in lines: if line.startswith(' '): assert test_suite_name is not None cases.append(test_suite_name + line.strip()) else: test_suite_name = line return cases def create_test_runner(self, workspace_dir: str, log_dir: str, model: str, case: str, config: Dict[str, Any], speed_factor: float, verbose: bool, build_dir: str) -> ph.Runner: return ph.TestRunnerRos( workspace_dir, log_dir, model, case, verbose, self._ros_package_build_dir) def rootfs_base_dirname(self) -> str: return "tmp_ros_tests" def create_argument_parser() -> argparse.ArgumentParser: parser = argparse.ArgumentParser() parser.add_argument("--list-cases", action='store_true', help="List available test cases") parser.add_argument("--log-dir", help="Directory for log files", default="logs") parser.add_argument("--iterations", type=int, default=1, help="how often to run all tests") parser.add_argument("--abort-early", action='store_true', help="abort on first unsuccessful test") parser.add_argument("--gui", default=False, action='store_true', help="display the visualization for a simulation") parser.add_argument("--model", type=str, default='all', help="only run tests for one model") parser.add_argument("--case", type=str, default='all', help="only run tests for one case " "(or multiple cases with wildcard '*')") parser.add_argument("--debugger", default="", help="choice from valgrind, callgrind, gdb, lldb") parser.add_argument("--upload", default=False, action='store_true', help="Upload logs to logs.px4.io") parser.add_argument("--force-color", default=False, action='store_true', help="Force colorized output") parser.add_argument("--verbose", default=False, action='store_true', help="enable more verbose output") parser.add_argument("--config-file", help="JSON config file to use " "(default: test/ros_tests/config-sih.json)", default="test/ros_tests/config-sih.json") parser.add_argument("--build-dir", type=str, default='build/px4_sitl_sih/', help="relative path where the built files are stored " "(default: build/px4_sitl_sih/)") parser.add_argument("--px4-ros2-interface-lib-build-dir", type=str, 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 def main() -> NoReturn: parser = create_argument_parser() args = parser.parse_args() if args.force_color: logger_helper.force_color = True ros_package_build_dir = args.px4_ros2_interface_lib_build_dir if ros_package_build_dir is None: ros_package_build_dir = lookup_px4_ros2_cpp_build_dir() if args.verbose: print(f'px4_ros2_cpp build dir: {ros_package_build_dir}') tester_interface = TesterInterfaceRos(ros_package_build_dir) if args.list_cases: for case in tester_interface.query_test_cases(args.build_dir, '*'): print(f'{case}') sys.exit(0) with open(args.config_file) as json_file: config = json.load(json_file) if config["mode"] != "sitl" and args.gui: print("--gui is not compatible with the mode '{}'" .format(config["mode"])) sys.exit(1) if config.get("simulator") == "sih" and args.gui: parser.error("--gui is not supported by the SIH backend") if not is_everything_ready(config, args.build_dir): sys.exit(1) if args.verbose: print("Creating directory: {}".format(args.log_dir)) os.makedirs(args.log_dir, exist_ok=True) speed_factor = 1 # Not (yet) supported tester = test_runner.Tester( config, args.iterations, args.abort_early, speed_factor, args.model, args.case, args.debugger, args.log_dir, args.gui, args.verbose, args.upload, args.build_dir, tester_interface ) empty_filters = [test['test_filter'] for test in tester.tests if test['selected'] and not test['cases']] if empty_filters: parser.error("Configured test filters matched no cases: " + ", ".join(empty_filters)) if tester.num_cases() == 0: parser.error("No test cases selected; check --config-file, --model and --case") signal.signal(signal.SIGINT, tester.sigint_handler) # PX4 and the test binary inherit the environment through process_helper.Runner os.environ.update(middleware_environment(args.middleware)) # 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() def restart_bridge(_model: str, _case: str) -> None: bridge.restart() tester.pre_test_hook = restart_bridge try: result = tester.run() finally: bridge.stop_process_if_started() sys.exit(0 if result else 1) def is_running(process_name: str) -> bool: for proc in psutil.process_iter(attrs=['name']): if proc.info['name'] == process_name: return True return False def is_everything_ready(config: Dict[str, str], build_dir: str) -> bool: result = True if config['mode'] == 'sitl': if is_running('px4'): print("px4 process already running\n" "run `killall px4` and try again") result = False if not os.path.isfile(os.path.join(build_dir, 'bin/px4')): if config.get('simulator') == 'sih': print("PX4 SIH SITL is not built\n" "run `make px4_sitl_sih` and use " "`--build-dir build/px4_sitl_sih`") else: print("PX4 SITL is not built\n" "run `DONT_RUN=1 make px4_sitl gazebo` or " "`DONT_RUN=1 make px4_sitl_default gazebo`") result = False if config.get('simulator') == 'gazebo': if is_running('gzserver'): print("gzserver process already running\n" "run `killall gzserver` and try again") result = False if is_running('gzclient'): print("gzclient process already running\n" "run `killall gzclient` and try again") result = False return result def lookup_px4_ros2_cpp_build_dir(): """Get the ROS2 build directory for px4_ros2_cpp""" try: p = subprocess.Popen( 'ros2 pkg prefix px4_ros2_cpp'.split(), stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) except FileNotFoundError: print('Error: command "ros2" not found. Did you source the ros workspace?') sys.exit(1) assert p.stdout is not None lines = str(p.stdout.read().decode("utf-8")).strip().split('\n') assert len(lines) == 1 return os.path.join(lines[0], '../../build/px4_ros2_cpp') if __name__ == '__main__': main()