Add Battery State Broadcaster controller

battery_state_broadcaster/CMakeLists.txt

@@ -0,0 +1,94 @@
+cmake_minimum_required(VERSION 3.8)
+project(battery_state_broadcaster)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  builtin_interfaces
+  control_msgs
+  controller_interface
+  generate_parameter_library
+  pluginlib
+  rclcpp_lifecycle
+  realtime_tools
+  sensor_msgs
+  control_msgs
+  urdf
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(battery_state_broadcaster_parameters
+  src/battery_state_broadcaster.yaml
+)
+
+add_library(battery_state_broadcaster SHARED
+  src/battery_state_broadcaster.cpp
+)
+
+target_compile_features(battery_state_broadcaster PUBLIC cxx_std_17)
+target_include_directories(battery_state_broadcaster
+  PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/battery_state_broadcaster>
+)
+target_link_libraries(battery_state_broadcaster PUBLIC
+                      battery_state_broadcaster_parameters
+                      controller_interface::controller_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${sensor_msgs_TARGETS}
+                      ${control_msgs_TARGETS}
+                      ${builtin_interfaces_TARGETS})
+
+
+pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_battery_state_broadcaster test/test_load_battery_state_broadcaster.cpp)
+  target_include_directories(test_load_battery_state_broadcaster PRIVATE include)
+  target_link_libraries(test_load_battery_state_broadcaster
+    battery_state_broadcaster
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(test_battery_state_broadcaster
+    test/test_battery_state_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/battery_state_broadcaster_params.yaml)
+  target_include_directories(test_battery_state_broadcaster PRIVATE include)
+  target_link_libraries(test_battery_state_broadcaster
+    battery_state_broadcaster
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/battery_state_broadcaster
+)
+install(
+  TARGETS
+    battery_state_broadcaster
+    battery_state_broadcaster_parameters
+  EXPORT export_battery_state_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_battery_state_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

battery_state_broadcaster/battery_state_broadcaster.xml

@@ -0,0 +1,26 @@
+<!--
+Copyright (c) 2025, b-robotized Group
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+-->
+
+<library path="battery_state_broadcaster">
+  <class name="battery_state_broadcaster/BatteryStateBroadcaster"
+         type="battery_state_broadcaster::BatteryStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    This controller publishes the individual battery state of each joint as sensor_msgs/BatteryState messages.
+    It also publishes the aggregated battery state of all joints as a single control_msgs/BatteryStates message.
+  </description>
+  </class>
+</library>

battery_state_broadcaster/doc/userdoc.rst

@@ -0,0 +1,88 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/battery_state_broadcaster/doc/userdoc.rst
+
+.. _battery_state_broadcaster_userdoc:
+
+Battery State Broadcaster
+--------------------------------
+The *Battery State Broadcaster* is a ROS 2 controller that publishes battery status information as ``sensor_msgs/msg/BatteryState`` messages.
+
+It reads battery-related state interfaces from one or more joints and exposes them in a standard ROS 2 message format. This allows easy integration with monitoring tools, logging systems, and higher-level decision-making nodes.
+
+Interfaces
+^^^^^^^^^^^
+
+The broadcaster can read the following state interfaces from each configured joint:
+
+- ``battery_voltage`` *(mandatory)* (double)
+- ``battery_temperature`` *(optional)* (double)
+- ``battery_current`` *(optional)* (double)
+- ``battery_charge`` *(optional)* (double)
+- ``battery_percentage`` *(optional)* (double)
+- ``battery_power_supply_status`` *(optional)* (double)
+- ``battery_power_supply_health`` *(optional)* (double)
+- ``battery_present`` *(optional)* (bool)
+
+Published Topics
+^^^^^^^^^^^^^^^^^^
+
+The broadcaster publishes two topics:
+
+- ``~/raw_battery_states`` (``control_msgs/msg/BatteryStates``)
+  Publishes **per-joint battery state messages**, containing the raw values for each configured joint.
+
+- ``~/battery_state`` (``sensor_msgs/msg/BatteryState``)
+  Publishes a **single aggregated battery message** representing the combined status across all joints.
+
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| Field                       | ``battery_state``                                                    | ``raw_battery_states``                                                                                                                      |
++=============================+======================================================================+=============================================================================================================================================+
+| ``header.frame_id``         | Empty                                                                | Joint name                                                                                                                                  |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``voltage``                 | Mean across all joints                                               | From joint's ``battery_voltage`` interface if enabled, otherwise nan                                                                        |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``temperature``             | Mean across joints reporting temperature                             | From joint's ``battery_temperature`` interface if enabled, otherwise nan.                                                                   |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``current``                 | Mean across joints reporting current                                 | From joint's ``battery_current`` interface if enabled, otherwise nan.                                                                       |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``charge``                  | Sum across all joints                                                | From joint's ``battery_charge`` interface if enabled, otherwise nan.                                                                        |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``capacity``                | Sum across all joints                                                | From joint's ``capacity`` parameter if provided, otherwise nan.                                                                             |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``design_capacity``         | Sum across all joints                                                | From joint's ``design_capacity`` parameter if provided, otherwise nan.                                                                      |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``percentage``              | Mean across joints reporting/calculating percentage                  | From joint's ``battery_percentage`` interface if enabled, otherwise calculated from joint's ``min_voltage`` and ``max_voltage`` parameters. |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``power_supply_status``     | Highest reported enum value                                          | From joint's ``battery_power_supply_status`` interface if enabled, otherwise 0 (unknown).                                                   |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``power_supply_health``     | Highest reported enum value                                          | From joint's ``battery_power_supply_health`` interface if enabled, otherwise 0 (unknown).                                                   |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``power_supply_technology`` | Reported as-is if same across all joints, otherwise set to *Unknown* | From joint's ``power_supply_technology`` parameter if provided, otherwise 0 (unknown).                                                      |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``present``                 | True                                                                 | From joint's ``battery_present`` interface if enabled, otherwise true if joint's voltage values is valid.                                   |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``cell_voltage``            | Empty                                                                | Empty                                                                                                                                       |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``cell_temperature``        | Empty                                                                | Empty                                                                                                                                       |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``location``                | All joint locations appended                                         | From joint's ``location`` parameter if provided, otherwise empty.                                                                           |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+| ``serial_number``           | All joint serial numbers appended                                    | From joint's ``serial_number`` parameter if provided, otherwise empty.                                                                      |
++-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
+
+
+Parameters
+^^^^^^^^^^^
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to manage parameters.
+The parameter `definition file <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml>`_ contains the full list and descriptions.
+
+List of parameters
+=========================
+.. generate_parameter_library_details:: ../src/battery_state_broadcaster_parameters.yaml
+
+Example Parameter File
+=========================
+
+An example parameter file for this controller is available in the `test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml>`_:
+
+.. literalinclude:: ../test/battery_state_broadcaster_params.yaml
+   :language: yaml

battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp

@@ -0,0 +1,137 @@
+// Copyright (c) 2025, b-robotized Group
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_
+#define BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+
+#include <battery_state_broadcaster/battery_state_broadcaster_parameters.hpp>
+#include "control_msgs/msg/battery_states.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+
+namespace battery_state_broadcaster
+{
+// // name constants for state interfaces
+// static constexpr size_t STATE_MY_ITFS = 0;
+
+// // name constants for command interfaces
+// static constexpr size_t CMD_MY_ITFS = 0;
+
+/**
+ * \brief Battery State Broadcaster for all or some state in a ros2_control system.
+ *
+ * BatteryStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
+ * The following state interfaces are published:
+ *    <state_joint>/voltage
+ *    <state_joint>/current
+ *    <state_joint>/charge
+ *    <state_joint>/percentage
+ *    <state_joint>/power_supply_status
+ *    <state_joint>/power_supply_health
+ *    <state_joint>/present
+ *
+ * \param state_joints of the batteries to publish.
+ * \param capacity of the batteries to publish.
+ * \param design_capacity of the batteries to publish.
+ * \param power_supply_technology of the batteries to publish.
+ * \param location of the batteries to publish.
+ * \param serial_number of the batteries to publish.
+ *
+ * Publishes to:
+ *
+ * - \b battery_state (sensor_msgs::msg::BatteryState): battery state of the combined battery
+ * joints.
+ * - \b raw_battery_states (battery_state_broadcaster::msg::BatteryStates): battery states of the
+ * individual battery joints.
+ *
+ */
+class BatteryStateBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  BatteryStateBroadcaster();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  float get_or_nan(int interface_cnt);
+  char get_or_unknown(int interface_cnt);
+
+protected:
+  std::shared_ptr<battery_state_broadcaster::ParamListener> param_listener_;
+  battery_state_broadcaster::Params params_;
+
+  std::vector<std::string> state_joints_;
+
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::BatteryState>> battery_state_publisher_;
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::BatteryStates>>
+    raw_battery_states_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>>
+    battery_state_realtime_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::BatteryStates>>
+    raw_battery_states_realtime_publisher_;
+  struct BatteryInterfaceSums
+  {
+    float voltage_sum = 0.0f;
+    float temperature_sum = 0.0f;
+    float current_sum = 0.0f;
+    float charge_sum = 0.0f;
+    float percentage_sum = 0.0f;
+    float capacity_sum = 0.0f;
+    float design_capacity_sum = 0.0f;
+  };
+
+  struct BatteryInterfaceCounts
+  {
+    float temperature_cnt = 0.0f;
+    float current_cnt = 0.0f;
+    float percentage_cnt = 0.0f;
+  };
+
+  BatteryInterfaceSums sums_;
+  BatteryInterfaceCounts counts_;
+
+  std::vector<bool> battery_presence_;
+
+private:
+};
+
+}  // namespace battery_state_broadcaster
+
+#endif  // BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_