mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
The runner could only drive tests through uXRCE-DDS. Add --middleware zenoh: it manages the rmw_zenohd router instead of the Agent and selects Zenoh in both PX4 and the test binary through the environment. The router is restarted between tests because every SITL instance announces the same node GUID, so stale liveliness tokens would let the next test see the FMU before it is up. config-sih-zenoh.json limits the Zenoh run to the mode tests: Zenoh does not synchronize timestamps with ROS yet, so EKF2 rejects the position interface inputs. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
382 lines
14 KiB
Python
Executable File
382 lines
14 KiB
Python
Executable File
#!/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()
|