Add Battery State Broadcaster controller

battery_state_broadcaster/CMakeLists.txt

@@ -1,54 +1,100 @@
 cmake_minimum_required(VERSION 3.8)
 project(battery_state_broadcaster)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
+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 dependencies
 find_package(ament_cmake REQUIRED)
-find_package(controller_interface REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(realtime_tools REQUIRED)
-find_package(sensor_msgs 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})
 
-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>
+generate_parameter_library(battery_state_broadcaster_parameters
+  src/battery_state_broadcaster.yaml
+)
+
+add_library(
+  battery_state_broadcaster
+  SHARED
+  src/battery_state_broadcaster.cpp
 )
-target_link_libraries(battery_state_broadcaster
+
+target_compile_features(battery_state_broadcaster PUBLIC cxx_std_17)
+target_include_directories(battery_state_broadcaster
   PUBLIC
-    controller_interface::controller_interface
-    pluginlib::pluginlib
-    realtime_tools::realtime_tools
-    ${sensor_msgs_TARGETS}
+  $<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)
 
-pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
+  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
+    battery_state_broadcaster_parameters
   EXPORT export_battery_state_broadcaster
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
-  INCLUDES DESTINATION include
 )
 
 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

@@ -1,8 +1,9 @@
 <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 readings of a battery sensor as sensor_msgs/BatteryState message.
-    </description>
-    </class>
-  </library>
+  <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/BatteryStateArray message.
+  </description>
+  </class>
+</library>

battery_state_broadcaster/doc/userdoc.rst

@@ -3,53 +3,123 @@
 .. _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`` (``control_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
 =========================
-This broadcaster publishes `sensor_msgs/BatteryState <https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg>`__ messages from appropriate state interfaces.
+.. generate_parameter_library_details:: ../src/battery_state_broadcaster_parameters.yaml
 
+Example Parameter File
+=========================
 
-Required State Interfaces
----------------------------------
-This broadcaster requires the robot to have a sensor component which contains the battery state interfaces:
+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>`_:
 
-.. code-block:: xml
+.. literalinclude:: ../test/battery_state_broadcaster_params.yaml
+   :language: yaml
 
-  <ros2_control type="system">
+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.
 
-      <sensor name="battery_state">
-          <state_interface name="voltage" />
-      </sensor>
-  </ros2_control>
+To adapt your setup to the new ``battery_state_broadcaster`` configuration:
 
-Parameters
----------------------------------
-To use this broadcaster, declare it in the controller manager and set its parameters:
+1. Update your hardware interface name from ``voltage`` → ``battery_voltage``.
+
+2. Convert your controller parameters from
 
-.. code-block:: yaml
+  .. code-block:: yaml
 
-  controller_manager:
-    ros__parameters:
-      battery_state_broadcaster:
-        type: battery_state_broadcaster/BatteryStateBroadcaster
+    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
 
-  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:
 
-And spawn it in the launch file:
+  .. code-block:: yaml
 
-.. code-block:: python
+    battery_state_broadcaster:
+      ros__parameters:
+        state_joints: ["battery_state"]
+        battery_state:
+          design_capacity: 100.0
+          power_supply_technology: 2
 
-  battery_state_broadcaster_spawner = Node(
-      package="controller_manager",
-      executable="spawner",
-      arguments=["battery_state_broadcaster"]
+**Notes**:
 
-Topics
----------------------------------
-The battery state is published on ``~/battery_state``.
-Since it's a plugin within the controller manager, add a remapping of the form ``("~/battery_state", "/my_battery_state")`` to the *controller manager*, not the spawner, to change the topic name.
+- 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.