Changes per Issue 2 - Handling more State Interfaces

battery_state_broadcaster/CMakeLists.txt

@@ -14,15 +14,23 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
 foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${dependency} REQUIRED)
 endforeach()
 
+# Parameters Library ================================================
+set(TARGET ${PROJECT_NAME}_parameters)
+generate_parameter_library(${TARGET} src/battery_state_broadcaster_parameters.yaml)
+
 add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp)
 target_include_directories(battery_state_broadcaster PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/battery_state_broadcaster>
 )
+
+target_link_libraries(${PROJECT_NAME} PUBLIC battery_state_broadcaster_parameters)
 ament_target_dependencies(battery_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
@@ -33,6 +41,7 @@ install(
 )
 install(
   TARGETS
+  battery_state_broadcaster_parameters
   battery_state_broadcaster
   EXPORT export_battery_state_broadcaster
   RUNTIME DESTINATION bin

battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp

@@ -4,15 +4,24 @@
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <sensor_msgs/msg/detail/battery_state__struct.hpp>
 
+//
+// inspired by
+// https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/include/semantic_components/battery_state.hpp
+//
+
 namespace battery_state_broadcaster
 {
 class BatterySensor : public semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>
 {
 public:
-  explicit BatterySensor(const std::string& name)
-    : semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>(name, 1)
+  explicit BatterySensor(const std::string& name, const std::vector<std::string>& interfaces)
+    : semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>(name, interfaces.size())
   {
-    interface_names_.emplace_back(name_ + "/" + "voltage");
+    for (const auto& interface : interfaces)
+    {
+      std::string interface_name = name_ + "/" + interface;
+      interface_names_.emplace_back(interface_name);
+    }
   }
 
   virtual ~BatterySensor() = default;
@@ -23,11 +32,50 @@ class BatterySensor : public semantic_components::SemanticComponentInterface<sen
     return voltage_;
   }
 
-  bool get_values_as_message(sensor_msgs::msg::BatteryState& message)
+  void populate_message_from_interfaces(sensor_msgs::msg::BatteryState& message)
   {
-    get_voltage();
-    message.voltage = static_cast<float>(voltage_);
-    return true;
+    // seed the message with dynamic values - only from existing interfaces:
+    for (const auto& state_interface : state_interfaces_)
+    {
+      const auto& name = state_interface.get().get_interface_name();
+      const auto& value = state_interface.get().get_value();
+      if (name == "voltage")
+      {
+        message.voltage = value;
+      }
+      else if (name == "temperature")
+      {
+        message.temperature = value;
+      }
+      else if (name == "charge")
+      {
+        message.charge = value;
+      }
+      else if (name == "current")
+      {
+        message.current = value;
+      }
+      else if (name == "capacity")
+      {
+        message.capacity = value;
+      }
+      else if (name == "percentage")
+      {
+        message.percentage = value;
+      }
+      else if (name == "power_supply_health")
+      {
+        message.power_supply_health = static_cast<uint8_t>(std::round(value));
+      }
+      else if (name == "power_supply_status")
+      {
+        message.power_supply_status = static_cast<uint8_t>(std::round(value));
+      }
+      else if (name == "present")
+      {
+        message.present = static_cast<int>(std::round(value)) == 0 ? false : true;
+      }
+    }
   }
 
 private:

battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp

@@ -5,6 +5,9 @@
 #include <realtime_tools/realtime_publisher.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
 
+// auto-generated by generate_parameter_library
+#include "battery_state_broadcaster/battery_state_broadcaster_parameters.hpp"
+
 #include "BatterySensor.hpp"
 
 namespace battery_state_broadcaster
@@ -27,8 +30,13 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface
   controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
 private:
-  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_pub_;
   std::unique_ptr<BatterySensor> battery_sensor_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>> realtime_publisher_;
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>;
+  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_pub_;
+  std::unique_ptr<StatePublisher> realtime_publisher_;
+
 };
 }  // namespace battery_state_broadcaster

battery_state_broadcaster/package.xml

@@ -3,13 +3,14 @@
 <package format="3">
   <name>battery_state_broadcaster</name>
   <version>1.0.1</version>
-  <description>ROS2 Control boradcaster for battery state sensors.</description>
+  <description>ROS2 Control broadcaster for battery state sensors.</description>
   <maintainer email="[email protected]">Jonas Otto</maintainer>
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
   <depend>sensor_msgs</depend>

battery_state_broadcaster/src/BatteryStateBroadcaster.cpp

@@ -3,13 +3,26 @@
 #include <rclcpp/logging.hpp>
 #include <rclcpp/qos.hpp>
 
+//
+// Some code from
+// https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster.cpp
+//
+
 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
+  {
+    param_listener_ = std::make_shared<ParamListener>(get_node());
+    params_ = param_listener_->get_params();
+  }
+  catch (const std::exception& e)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -18,35 +31,46 @@ BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_
 {
   std::string sensor_name = get_node()->get_parameter("sensor_name").as_string();
 
-  battery_sensor_ = std::make_unique<BatterySensor>(BatterySensor(sensor_name));
+  params_ = param_listener_->get_params();
 
-  battery_state_pub_ =
-      get_node()->create_publisher<sensor_msgs::msg::BatteryState>("~/battery_state", rclcpp::SystemDefaultsQoS());
-  realtime_publisher_ =
-      std::make_unique<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>>(battery_state_pub_);
+  battery_sensor_ = std::make_unique<BatterySensor>(BatterySensor(sensor_name, params_.state_interfaces));
 
-  realtime_publisher_->msg_.temperature = std::numeric_limits<double>::quiet_NaN();
-  realtime_publisher_->msg_.current = std::numeric_limits<double>::quiet_NaN();
-  realtime_publisher_->msg_.charge = std::numeric_limits<double>::quiet_NaN();
-  realtime_publisher_->msg_.capacity = std::numeric_limits<double>::quiet_NaN();
-  realtime_publisher_->msg_.design_capacity = std::numeric_limits<double>::quiet_NaN();
-  realtime_publisher_->msg_.percentage = std::numeric_limits<double>::quiet_NaN();
+  try
+  {
+    // register ft sensor data publisher
+    battery_state_pub_ =
+        get_node()->create_publisher<sensor_msgs::msg::BatteryState>("~/battery_state", rclcpp::SystemDefaultsQoS());
+    realtime_publisher_ = std::make_unique<StatePublisher>(battery_state_pub_);
+  }
+  catch (const std::exception& e)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(),
+                 "Exception thrown during publisher creation at configure stage with message : %s \n",
+                 e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  realtime_publisher_->lock();
+
+  // seed the message with static values from parameters and defaults for dynamic members:
+  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
+  realtime_publisher_->msg_.design_capacity = params_.design_capacity;
+  realtime_publisher_->msg_.voltage = std::numeric_limits<float>::quiet_NaN();
+  realtime_publisher_->msg_.temperature = std::numeric_limits<float>::quiet_NaN();
+  realtime_publisher_->msg_.charge = std::numeric_limits<float>::quiet_NaN();
+  realtime_publisher_->msg_.current = std::numeric_limits<float>::quiet_NaN();
+  realtime_publisher_->msg_.capacity = std::numeric_limits<float>::quiet_NaN();
+  realtime_publisher_->msg_.percentage = std::numeric_limits<float>::quiet_NaN();
   realtime_publisher_->msg_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
   realtime_publisher_->msg_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
-  realtime_publisher_->msg_.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
+  realtime_publisher_->msg_.power_supply_technology = params_.power_supply_technology;
   realtime_publisher_->msg_.present = true;
+  realtime_publisher_->msg_.location = params_.location;
+  realtime_publisher_->msg_.serial_number = params_.serial_number;
 
-  int64_t psu_tech = get_node()->get_parameter("power_supply_technology").as_int();
-  if (psu_tech != -1)
-  {
-    realtime_publisher_->msg_.power_supply_technology = psu_tech;
-  }
+  realtime_publisher_->unlock();
 
-  double design_capacity = get_node()->get_parameter("design_capacity").as_double();
-  if (design_capacity != 0.0)
-  {
-    realtime_publisher_->msg_.design_capacity = static_cast<float>(design_capacity);
-  }
+  RCLCPP_DEBUG(get_node()->get_logger(), "on_configure() successful");
 
   return CallbackReturn::SUCCESS;
 }
@@ -87,7 +111,7 @@ controller_interface::return_type BatteryStateBroadcaster::update(const rclcpp::
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
     realtime_publisher_->msg_.header.stamp = time;
-    battery_sensor_->get_values_as_message(realtime_publisher_->msg_);
+    battery_sensor_->populate_message_from_interfaces(realtime_publisher_->msg_);
     realtime_publisher_->unlockAndPublish();
   }
 

battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml

@@ -0,0 +1,53 @@
+#
+# inspired by https://github.com/HarvestX/h6x_ros2_controllers/blob/humble/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml
+#
+
+battery_state_broadcaster:
+  sensor_name:
+    type: string
+    description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined."
+    default_value: "battery_sensor"
+  frame_id:
+    type: string
+    description: "Sensor's frame_id in which values are published."
+    default_value: "base_link"
+  power_supply_technology:
+    type: int
+    description: "The battery chemistry."
+    default_value: 0
+    validation:
+  design_capacity:
+    type: double
+    description: "Capacity in in Ah (design capacity) (If unmeasured NaN)"
+    default_value: .NAN
+  location:
+    type: string
+    description: "The location into which the battery is inserted. (slot number or plug)"
+    default_value: ""
+  serial_number:
+    type: string
+    description: "The best approximation of the battery serial number"
+    default_value: ""
+
+  # the Base driver can report dynamic parameters:
+  state_interfaces:
+    type: string_array
+    default_value: ["voltage"] # voltage is mandatory
+    validation:
+      not_empty<>: []
+      size_lt<>: 10
+      subset_of<>: [
+          [
+            # Mandatory:
+            "voltage",
+            # Optional:
+            "temperature",
+            "current",
+            "charge",
+            "capacity",
+            "percentage",
+            "power_supply_status",
+            "power_supply_health",
+            "present",
+          ],
+        ]