mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
test(ros): run the ROS integration tests over Zenoh
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>
This commit is contained in:
1 parent
af00ee618a
commit
e64bad47d4
3 files changed
+147
-9
No files matched your search
+96
-9
@@ -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)
|
||||
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
{
|
||||
"mode": "sitl",
|
||||
"simulator": "sih",
|
||||
"model_prefix": "sihsim_",
|
||||
"tests":
|
||||
[
|
||||
{
|
||||
"model": "quadx",
|
||||
"vehicle": "quadx",
|
||||
"test_filter": "ModesTest.*",
|
||||
"timeout_min": 10
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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()
|
||||
Reference in new issue
Block a user