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

@@ -272,8 +272,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);
+ snprintf(msg.header.frame_id, 2, "%d", (int)instance);
+
  auto &battery = AP::battery();
 
  if (!battery.healthy(instance)) {
@@ -1151,10 +1154,13 @@ void AP_DDS_Client::update()
  }
 
  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();
  }
 
  if (cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) {

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -95,7 +95,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

@@ -172,7 +172,7 @@ $ ros2 node list
 ```bash
 $ ros2 topic list -v
 Published topics:
- * /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