From 8c1d957c4f9d477644459ed16f12fe01f5f65e2f Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 26 Nov 2023 09:48:36 -0700 Subject: [PATCH] Tools: ros2: Add geopose test * Add missing deps * Reduce some duplication Signed-off-by: Ryan Friedman --- Tools/ros2/ardupilot_dds_tests/package.xml | 3 +- .../test_geopose_msg_received.py | 150 ++++++++++++++++++ .../test_navsat_msg_received.py | 10 +- .../test_time_msg_received.py | 14 +- 4 files changed, 166 insertions(+), 11 deletions(-) create mode 100644 Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py diff --git a/Tools/ros2/ardupilot_dds_tests/package.xml b/Tools/ros2/ardupilot_dds_tests/package.xml index 528f4794a1052e..d7a6164c0549ee 100644 --- a/Tools/ros2/ardupilot_dds_tests/package.xml +++ b/Tools/ros2/ardupilot_dds_tests/package.xml @@ -27,7 +27,8 @@ ament_lint_auto ardupilot_msgs ardupilot_sitl - builtin_interfaces + builtin_interfaces + geographic_msgs launch launch_pytest launch_ros diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py new file mode 100644 index 00000000000000..dc68680bc5bb1c --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py @@ -0,0 +1,150 @@ +# Copyright 2023 ArduPilot.org. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +"""Bring up ArduPilot SITL and check the GeoPose message is being published.""" + +import launch_pytest +import pytest +import rclpy +import rclpy.node +import threading + +from launch import LaunchDescription + +from launch_pytest.tools import process as process_tools + +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSHistoryPolicy + +from geographic_msgs.msg import GeoPoseStamped + +TOPIC = "ap/geopose/filtered" + + +class GeoPoseListener(rclpy.node.Node): + """Subscribe to GeoPoseStamped messages""" + + def __init__(self): + """Initialise the node.""" + super().__init__("geopose_listener") + self.msg_event_object = threading.Event() + + # Declare and acquire `topic` parameter + self.declare_parameter("topic", TOPIC) + self.topic = self.get_parameter("topic").get_parameter_value().string_value + + def start_subscriber(self): + """Start the subscriber.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.subscription = self.create_subscription(GeoPoseStamped, self.topic, self.subscriber_callback, qos_profile) + + # Add a spin thread. + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def subscriber_callback(self, msg): + """Process a GeoPoseStamped message.""" + self.msg_event_object.set() + + position = msg.pose.position + + self.get_logger().info( + "From AP : Position [lat:{}, lon: {}]".format(position.latitude, position.longitude) + ) + +@launch_pytest.fixture +def launch_sitl_copter_dds_serial(sitl_copter_dds_serial): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_serial + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@launch_pytest.fixture +def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_udp + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_serial) +def test_dds_serial_geopose_msg_recv(launch_context, launch_sitl_copter_dds_serial): + """Test position messages are published by AP_DDS.""" + _, actions = launch_sitl_copter_dds_serial + virtual_ports = actions["virtual_ports"].action + micro_ros_agent = actions["micro_ros_agent"].action + mavproxy = actions["mavproxy"].action + sitl = actions["sitl"].action + + # Wait for process to start. + process_tools.wait_for_start_sync(launch_context, virtual_ports, timeout=2) + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) + + rclpy.init() + try: + node = GeoPoseListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=100.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + finally: + rclpy.shutdown() + yield + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_geopose_msg_recv(launch_context, launch_sitl_copter_dds_udp): + """Test position messages are published by AP_DDS.""" + _, actions = launch_sitl_copter_dds_udp + micro_ros_agent = actions["micro_ros_agent"].action + mavproxy = actions["mavproxy"].action + sitl = actions["sitl"].action + + # Wait for process to start. + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) + + rclpy.init() + try: + node = GeoPoseListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=100.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + finally: + rclpy.shutdown() + yield diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py index 05ff1343d271df..835b3e95d30e00 100644 --- a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py @@ -30,9 +30,11 @@ from sensor_msgs.msg import NavSatFix +TOPIC = "ap/navsat/navsat0" + class NavSatFixListener(rclpy.node.Node): - """Subscribe to NavSatFix messages on /ap/navsat/navsat0.""" + """Subscribe to NavSatFix messages.""" def __init__(self): """Initialise the node.""" @@ -40,7 +42,7 @@ def __init__(self): self.msg_event_object = threading.Event() # Declare and acquire `topic` parameter - self.declare_parameter("topic", "ap/navsat/navsat0") + self.declare_parameter("topic", TOPIC) self.topic = self.get_parameter("topic").get_parameter_value().string_value def start_subscriber(self): @@ -117,7 +119,7 @@ def test_dds_serial_navsat_msg_recv(launch_context, launch_sitl_copter_dds_seria node = NavSatFixListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ap/navsat/navsat0' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield @@ -141,7 +143,7 @@ def test_dds_udp_navsat_msg_recv(launch_context, launch_sitl_copter_dds_udp): node = NavSatFixListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ap/navsat/navsat0' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py index 5ffa1de6e0b11c..dc33b88c0e9ec3 100644 --- a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py @@ -26,9 +26,11 @@ from builtin_interfaces.msg import Time +TOPIC = "ap/time" + class TimeListener(rclpy.node.Node): - """Subscribe to Time messages on /ap/time.""" + """Subscribe to Time messages.""" def __init__(self): """Initialise the node.""" @@ -36,7 +38,7 @@ def __init__(self): self.msg_event_object = threading.Event() # Declare and acquire `topic` parameter. - self.declare_parameter("topic", "ap/time") + self.declare_parameter("topic", TOPIC) self.topic = self.get_parameter("topic").get_parameter_value().string_value def start_subscriber(self): @@ -89,7 +91,7 @@ def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): @pytest.mark.launch(fixture=launch_sitl_copter_dds_serial) def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial): - """Test /ap/time is published by AP_DDS.""" + """Test Time is published by AP_DDS.""" _, actions = launch_sitl_copter_dds_serial virtual_ports = actions["virtual_ports"].action micro_ros_agent = actions["micro_ros_agent"].action @@ -107,7 +109,7 @@ def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial) node = TimeListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ROS_Time' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield @@ -115,7 +117,7 @@ def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial) @pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) def test_dds_udp_time_msg_recv(launch_context, launch_sitl_copter_dds_udp): - """Test /ap/time is published by AP_DDS.""" + """Test Time is published by AP_DDS.""" _, actions = launch_sitl_copter_dds_udp micro_ros_agent = actions["micro_ros_agent"].action mavproxy = actions["mavproxy"].action @@ -131,7 +133,7 @@ def test_dds_udp_time_msg_recv(launch_context, launch_sitl_copter_dds_udp): node = TimeListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ROS_Time' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield