AP_DDS: battery topic to report all the available batteries

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 <https://www.gnu.org/licenses/>.
+
+"""
+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

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

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,

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