Add Battery State Broadcaster controller

battery_state_broadcaster/CMakeLists.txt

@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 3.8)
+project(battery_state_broadcaster)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  builtin_interfaces
+  controller_interface
+  hardware_interface
+  generate_parameter_library
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  sensor_msgs
+  urdf
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR})
+add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR})
+
+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
+                      hardware_interface::hardware_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${sensor_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 sensor_msgs/BatteryStateArray message.
+  </description>
+  </class>
+</library>

battery_state_broadcaster/doc/userdoc.rst

@@ -0,0 +1,125 @@
+: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* 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`` (``sensor_msgs/msg/BatteryStateArray``)
+  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
+
+Migration for ``ipa320/ros_battery_monitoring`` users
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If you were previously using the ``battery_state_broadcaster`` from the ``ipa320/ros_battery_monitoring package``, you can switch directly to this package. The configuration style using ``sensor_name`` is still supported for backward compatibility, but it may be removed in a future release.
+
+To adapt your setup to the new ``battery_state_broadcaster`` configuration:
+
+1. Update your hardware interface name from ``voltage`` → ``battery_voltage``.
+
+2. Convert your controller parameters from
+
+  .. code-block:: yaml
+
+    battery_state_broadcaster:
+      ros__parameters:
+        sensor_name: "battery_state"
+        design_capacity: 100.0
+        # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg
+        power_supply_technology: 2
+
+  to:
+
+  .. code-block:: yaml
+
+    battery_state_broadcaster:
+      ros__parameters:
+        state_joints: ["battery_state"]
+        battery_state:
+          design_capacity: 100.0
+          power_supply_technology: 2
+
+**Notes**:
+
+- Parameters must provide **either** sensor_name **or** state_joints.
+- If both are empty → the broadcaster will fail to configure.
+- If both are set → the broadcaster will throw an error.