AP_DDS: Integrate AP_BattMonitor to work with AP_DDS

* Edit BatteryState.idl
* Add BatteryState to AP_DDS_Topic_table.h
* Add BatteryState to the DDS Client
	* Add voltage
	* Add temperature
	* Add current
	* Add charge
	* Add capacity with NAN value
	* Add design_capacity
	* Add percentage
	* Add power_supply_status
	* Add power_supply_health
	* Add power_supply_technology with 0 value
	* Add present
	* Cell_voltage and Serial_number need to be implemented in the future
	* Did not add cell_temperature as AP_BattMonitor doesn't support it
	* Did not add location as this is a generic implementation
* Parameterize battery instance number

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -8,12 +8,13 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
 
 #include "AP_DDS_Client.h"
 
-
 static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
 static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000;
+static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
 static char WGS_84_FRAME_ID[] = "WGS-84";
 // https://www.ros.org/reps/rep-0105.html#base-link
 static char BASE_LINK_FRAME_ID[] = "base_link";
@@ -160,6 +161,78 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
 
 }
 
+void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance)
+{
+    if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
+        return;
+    }
+
+    update_topic(msg.header.stamp);
+    auto &battery = AP::battery();
+
+    if (!battery.healthy(instance))
+    {
+        msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD
+        msg.present = false;
+        return;
+    }
+    msg.present = true;
+
+    msg.voltage = battery.voltage(instance);
+
+    float temperature;
+    msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN;
+
+    float current;
+    msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN;
+
+    const float design_capacity = (float)battery.pack_capacity_mah(instance)/1000.0;
+    msg.design_capacity = design_capacity;
+
+    uint8_t percentage;
+    if (battery.capacity_remaining_pct(percentage, instance))
+    {
+        msg.percentage = percentage/100.0;
+        msg.charge = (percentage * design_capacity)/100.0;
+    }
+    else
+    {
+        msg.percentage = NAN;
+        msg.charge = NAN;
+    }
+
+    msg.capacity = NAN;
+
+    if (battery.current_amps(current, instance))
+    {
+        if (percentage == 100) {
+            msg.power_supply_status = 4;   //POWER_SUPPLY_STATUS_FULL
+        }
+        else if (current < 0.0) {
+            msg.power_supply_status = 1;   //POWER_SUPPLY_STATUS_CHARGING
+        }
+        else if (current > 0.0) {
+            msg.power_supply_status = 2;   //POWER_SUPPLY_STATUS_DISCHARGING
+        }
+        else {
+            msg.power_supply_status = 3;   //POWER_SUPPLY_STATUS_NOT_CHARGING
+        }
+    }
+    else
+    {
+        msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN
+    }
+
+    msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD
+
+    msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN
+
+    if (battery.has_cell_voltages(instance))
+    {
+        const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells;
+        std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage);
+    }
+}
 
 /*
   class constructor
@@ -339,10 +412,24 @@ void AP_DDS_Client::write_static_transforms()
     }
 }
 
+void AP_DDS_Client::write_battery_state_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub;
+        const uint32_t topic_size = sensor_msgs_msg_BatteryState_size_of_topic(&battery_state_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[3].dw_id,&ub,topic_size);
+        const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+
 void AP_DDS_Client::update()
 {
     WITH_SEMAPHORE(csem);
-
     const auto cur_time_ms = AP_HAL::millis64();
 
     if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) {
@@ -358,6 +445,13 @@ void AP_DDS_Client::update()
         write_nav_sat_fix_topic();
     }
 
+    if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
+        constexpr uint8_t instance = 0;
+        update_topic(battery_state_topic, instance);
+        last_battery_state_time_ms = cur_time_ms;
+        write_battery_state_topic();
+    }
+
     connected = uxr_run_session_time(&session, 1);
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -9,6 +9,7 @@
 
 #include "sensor_msgs/msg/NavSatFix.h"
 #include "tf2_msgs/msg/TFMessage.h"
+#include "sensor_msgs/msg/BatteryState.h"
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -44,6 +45,7 @@ private:
     builtin_interfaces_msg_Time time_topic;
     sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
     tf2_msgs_msg_TFMessage static_transforms_topic;
+    sensor_msgs_msg_BatteryState battery_state_topic;
 
     HAL_Semaphore csem;
 
@@ -53,11 +55,14 @@ private:
     static void update_topic(builtin_interfaces_msg_Time& msg);
     static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance);
     static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg);
+    static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance);
 
     // The last ms timestamp AP_DDS wrote a Time message
     uint64_t last_time_time_ms;
     // The last ms timestamp AP_DDS wrote a NavSatFix message
     uint64_t last_nav_sat_fix_time_ms;
+    // The last ms timestamp AP_DDS wrote a BatteryState message
+    uint64_t last_battery_state_time_ms;
 
 
 public:
@@ -81,6 +86,8 @@ public:
     void write_nav_sat_fix_topic();
     //! @brief Serialize the static transforms and publish to the IO stream(s)
     void write_static_transforms();
+    //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s)
+    void write_battery_state_topic();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -1,7 +1,7 @@
 #include "builtin_interfaces/msg/Time.h"
 #include "sensor_msgs/msg/NavSatFix.h"
 #include "tf2_msgs/msg/TFMessage.h"
-
+#include "sensor_msgs/msg/BatteryState.h"
 
 #include "AP_DDS_Generic_Fn_T.h"
 #include "uxr/client/client.h"
@@ -42,4 +42,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic),
         .size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic),
     },
+    {
+        .topic_id = 0x04,
+        .pub_id = 0x04,
+        .dw_id = uxrObjectId{.id=0x04, .type=UXR_DATAWRITER_ID},
+        .topic_profile_label = "batterystate0__t",
+        .dw_profile_label = "batterystate0__dw",
+        .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_BatteryState_serialize_topic),
+        .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_BatteryState_deserialize_topic),
+        .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_BatteryState_size_of_topic),
+    },
 };

libraries/AP_DDS/Idl/sensor_msgs/msg/BatteryState.idl

@@ -6,6 +6,7 @@
 
 module sensor_msgs {
   module msg {
+    typedef float float__14[14];
     module BatteryState_Constants {
       @verbatim (language="comment", text=
         "Constants are chosen to match the enums in the linux kernel" "\n"        "defined in include/linux/power_supply.h as of version 3.7" "\n"        "The one difference is for style reasons the constants are" "\n"        "all uppercase not mixed case." "\n"        "Power supply status constants")
@@ -85,12 +86,12 @@ module sensor_msgs {
       @verbatim (language="comment", text=
         "An array of individual cell voltages for each cell in the pack" "\n"
         "If individual voltages unknown but number of cells known set each to NaN")
-      sequence<float> cell_voltage;
+      float__14 cell_voltage;
 
       @verbatim (language="comment", text=
         "An array of individual cell temperatures for each cell in the pack" "\n"
         "If individual temperatures unknown but number of cells known set each to NaN")
-      sequence<float> cell_temperature;
+      float__14 cell_temperature;
 
       @verbatim (language="comment", text=
         "The location into which the battery is inserted. (slot number or plug)")

libraries/AP_DDS/dds_xrce_profile.xml

@@ -86,4 +86,29 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="batterystate0__t">
+    <name>rt/ROS2_BatteryState0</name>
+    <dataType>sensor_msgs::msg::dds_::BatteryState_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>20</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="batterystate0__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>RELIABLE</kind>
+      </reliability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ROS2_BatteryState0</name>
+      <dataType>sensor_msgs::msg::dds_::BatteryState_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>20</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
 </profiles>