Proposed new branch Humble - Fix library to work with ROS2 Humble

battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp

@@ -13,6 +13,7 @@ class BatterySensor : public semantic_components::SemanticComponentInterface<sen
     : semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>(name, 1)
   {
     interface_names_.emplace_back(name_ + "/" + "voltage");
+    interface_names_.emplace_back(name_ + "/" + "current");
   }
 
   virtual ~BatterySensor() = default;
@@ -23,14 +24,69 @@ class BatterySensor : public semantic_components::SemanticComponentInterface<sen
     return voltage_;
   }
 
+  double get_current()
+  {
+    current_ = state_interfaces_[1].get().get_value();
+    return current_;
+  }
+
   bool get_values_as_message(sensor_msgs::msg::BatteryState& message)
   {
     get_voltage();
+    get_current();
+
     message.voltage = static_cast<float>(voltage_);
+    message.percentage = calculate_percentage();
+    message.current = static_cast<float>(current_);
+
     return true;
   }
 
+  float calculate_percentage()
+  {
+    if (voltage_ >= batteryTable.front().voltage) {
+      return 100.0f;
+    }
+    if (voltage_ <= batteryTable.back().voltage) {
+      return 0.0f;
+    }
+
+    for (size_t i = 0; i < batteryTable.size() -1 ; ++i) {
+      if (voltage_ <= batteryTable[i].voltage && voltage_ >= batteryTable[i + 1].voltage) {
+        float vHigh = batteryTable[i].voltage;
+        float pHigh = batteryTable[i].percentage;
+        float vLow = batteryTable[i + 1].voltage;
+        float pLow = batteryTable[i + 1].percentage;
+
+        // Linear interpolation
+        return pLow + (pHigh - pLow) * (voltage_ - vLow) / (vHigh - vLow);
+      }
+    }
+
+    return 0.0f; // Default return value if no match found
+  }
+
+
+
 private:
   double voltage_ = 0.0;
+  double current_ = 0.0;
+  double percentage_ = 0.0;
+
+  struct VoltageMap {
+    float voltage;
+    float percentage;
+  };
+
+  // Based on the 20°C - 30°C curve from your Haisic manual
+  const std::vector<VoltageMap> batteryTable = {
+      {13.4f, 100.0f},
+      {13.2f, 90.0f},
+      {13.1f, 70.0f}, // The start of the flat plateau
+      {13.0f, 40.0f}, // Mid-plateau
+      {12.8f, 20.0f}, // The "knee" where voltage drops fast
+      {12.0f, 10.0f}, // Critical level
+      {10.0f, 0.0f}   // Empty
+  };
 };
 }  // namespace battery_state_broadcaster

battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp

@@ -2,7 +2,7 @@
 
 #include <controller_interface/controller_interface.hpp>
 #include <rclcpp/publisher.hpp>
-#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
 
 #include "BatterySensor.hpp"
@@ -12,9 +12,9 @@ namespace battery_state_broadcaster
 class BatteryStateBroadcaster : public controller_interface::ControllerInterface
 {
 public:
-  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
 
-  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
   controller_interface::CallbackReturn on_init() override;
 

battery_state_broadcaster/src/BatteryStateBroadcaster.cpp

@@ -7,9 +7,25 @@ namespace battery_state_broadcaster
 {
 controller_interface::CallbackReturn BatteryStateBroadcaster::on_init()
 {
-  get_node()->declare_parameter("sensor_name", "battery_state");
-  get_node()->declare_parameter("power_supply_technology", -1);
-  get_node()->declare_parameter("design_capacity", 0.0);
+  try {
+    auto node = get_node();
+
+    if(!node->has_parameter("sensor_name")){
+      node->declare_parameter("sensor_name", "battery_state");
+    }
+    if(!node->has_parameter("power_supply_technology")){
+      node->declare_parameter("power_supply_technology", -1);
+    }
+    if(!node->has_parameter("design_capacity")){
+      node->declare_parameter("design_capacity", 0.0);
+    }
+  }
+  catch (const std::exception & e)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Parameter declaration failed: %s", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -51,15 +67,15 @@ BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_
   return CallbackReturn::SUCCESS;
 }
 
-[[nodiscard]] controller_interface::InterfaceConfiguration
+controller_interface::InterfaceConfiguration
 BatteryStateBroadcaster::command_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration command_interfaces_config;
   command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
   return command_interfaces_config;
 }
 
-[[nodiscard]] controller_interface::InterfaceConfiguration BatteryStateBroadcaster::state_interface_configuration() const
+controller_interface::InterfaceConfiguration BatteryStateBroadcaster::state_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration state_interfaces_config;
   state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;