From 969979cd17a2b78cb558ed90d1f442162e1f7ded Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Wed, 2 Oct 2024 15:10:37 -0700 Subject: [PATCH] AP_DDS: battery topic to report all the available batteries --- .../test_battery_msg_received.py | 173 ++++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 12 +- libraries/AP_DDS/AP_DDS_Topic_Table.h | 2 +- libraries/AP_DDS/README.md | 2 +- 4 files changed, 184 insertions(+), 5 deletions(-) create mode 100644 Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_battery_msg_received.py diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_battery_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_battery_msg_received.py new file mode 100644 index 0000000000..50a5a517c1 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_battery_msg_received.py @@ -0,0 +1,173 @@ +# 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 BatteryState message is being published. + +Checks whether a message is received and that only frame_id = '0' is received, +as SITL has only one battery available. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_battery_msg_received + +""" + +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 sensor_msgs.msg import BatteryState + +TOPIC = "ap/battery" + + +class BatteryListener(rclpy.node.Node): + """Subscribe to BatteryState messages.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("battery_listener") + self.msg_event_object = threading.Event() + self.frame_id_correct_object = threading.Event() + self.frame_id_incorrect_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(BatteryState, 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 BatteryState message.""" + self.msg_event_object.set() + + self.get_logger().info("From AP : ID {} Voltage {}".format(msg.header.frame_id, msg.voltage)) + + if msg.header.frame_id == '0': + self.frame_id_correct_object.set() + + if msg.header.frame_id == '1': + self.frame_id_incorrect_object.set() + + +@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_battery_msg_recv(launch_context, launch_sitl_copter_dds_serial): + """Test battery 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 = BatteryListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + battery_correct_flag = node.frame_id_correct_object.wait(timeout=10.0) + assert battery_correct_flag, f"Did not receive correct battery ID." + battery_incorrect_flag = not node.frame_id_incorrect_object.wait(timeout=10.0) + assert battery_correct_flag, f"Did received incorrect battery ID." + finally: + rclpy.shutdown() + yield + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_battery_msg_recv(launch_context, launch_sitl_copter_dds_udp): + """Test battery 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 = BatteryListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + battery_correct_flag = node.frame_id_correct_object.wait(timeout=10.0) + assert battery_correct_flag, f"Did not receive correct battery ID." + battery_incorrect_flag = not node.frame_id_incorrect_object.wait(timeout=10.0) + assert battery_correct_flag, f"Did received incorrect battery ID." + + finally: + rclpy.shutdown() + yield diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 5ef37ca248..9ba608c202 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -301,8 +301,11 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_ if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { return; } + static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 99, "AP_BATT_MONITOR_MAX_INSTANCES is greater than 99"); update_topic(msg.header.stamp); + hal.util->snprintf(msg.header.frame_id, 2, "%u", instance); + auto &battery = AP::battery(); if (!battery.healthy(instance)) { @@ -1262,10 +1265,13 @@ void AP_DDS_Client::update() #endif // AP_DDS_NAVSATFIX_PUB_ENABLED #if AP_DDS_BATTERY_STATE_PUB_ENABLED if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { - constexpr uint8_t battery_instance = 0; - update_topic(battery_state_topic, battery_instance); + for (uint8_t battery_instance = 0; battery_instance < AP_BATT_MONITOR_MAX_INSTANCES; battery_instance++) { + update_topic(battery_state_topic, battery_instance); + if (battery_state_topic.present) { + write_battery_state_topic(); + } + } last_battery_state_time_ms = cur_time_ms; - write_battery_state_topic(); } #endif // AP_DDS_BATTERY_STATE_PUB_ENABLED #if AP_DDS_LOCAL_POSE_PUB_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 05d68e0c78..92d546dd51 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -132,7 +132,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAREADER_ID}, .topic_rw = Topic_rw::DataWriter, - .topic_name = "rt/ap/battery/battery0", + .topic_name = "rt/ap/battery", .type_name = "sensor_msgs::msg::dds_::BatteryState_", .qos = { .durability = UXR_DURABILITY_VOLATILE, diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 3a037f9b97..0284856597 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -173,7 +173,7 @@ $ ros2 node list $ ros2 topic list -v Published topics: * /ap/airspeed [geometry_msgs/msg/Vector3] 1 publisher - * /ap/battery/battery0 [sensor_msgs/msg/BatteryState] 1 publisher + * /ap/battery [sensor_msgs/msg/BatteryState] 1 publisher * /ap/clock [rosgraph_msgs/msg/Clock] 1 publisher * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher * /ap/gps_global_origin/filtered [geographic_msgs/msg/GeoPointStamped] 1 publisher