Skip to content

Commit 860e8ea

Browse files
author
Yara Shahin
committed
Add battery_state_broadcaster
1 parent dca8f45 commit 860e8ea

12 files changed

+1612
-0
lines changed

‎battery_state_broadcaster/CMakeLists.txt‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(battery_state_broadcaster)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
6+
endif()
7+
8+
set(THIS_PACKAGE_INCLUDE_DEPENDS
9+
builtin_interfaces
10+
control_msgs
11+
controller_interface
12+
generate_parameter_library
13+
pluginlib
14+
rclcpp_lifecycle
15+
realtime_tools
16+
sensor_msgs
17+
control_msgs
18+
urdf
19+
)
20+
21+
find_package(ament_cmake REQUIRED)
22+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
23+
find_package(${Dependency} REQUIRED)
24+
endforeach()
25+
26+
generate_parameter_library(battery_state_broadcaster_parameters
27+
src/battery_state_broadcaster.yaml
28+
)
29+
30+
add_library(battery_state_broadcaster SHARED
31+
src/battery_state_broadcaster.cpp
32+
)
33+
34+
target_compile_features(battery_state_broadcaster PUBLIC cxx_std_17)
35+
target_include_directories(battery_state_broadcaster
36+
PUBLIC
37+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
38+
$<INSTALL_INTERFACE:include/battery_state_broadcaster>
39+
)
40+
target_link_libraries(battery_state_broadcaster PUBLIC
41+
battery_state_broadcaster_parameters
42+
controller_interface::controller_interface
43+
pluginlib::pluginlib
44+
rclcpp::rclcpp
45+
rclcpp_lifecycle::rclcpp_lifecycle
46+
realtime_tools::realtime_tools
47+
${sensor_msgs_TARGETS}
48+
${control_msgs_TARGETS}
49+
${builtin_interfaces_TARGETS})
50+
51+
52+
pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
53+
54+
if(BUILD_TESTING)
55+
find_package(ament_cmake_gmock REQUIRED)
56+
find_package(controller_manager REQUIRED)
57+
find_package(hardware_interface REQUIRED)
58+
find_package(ros2_control_test_assets REQUIRED)
59+
60+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
61+
ament_add_gmock(test_load_battery_state_broadcaster test/test_load_battery_state_broadcaster.cpp)
62+
target_include_directories(test_load_battery_state_broadcaster PRIVATE include)
63+
target_link_libraries(test_load_battery_state_broadcaster
64+
battery_state_broadcaster
65+
controller_manager::controller_manager
66+
ros2_control_test_assets::ros2_control_test_assets
67+
)
68+
69+
add_rostest_with_parameters_gmock(test_battery_state_broadcaster
70+
test/test_battery_state_broadcaster.cpp
71+
${CMAKE_CURRENT_SOURCE_DIR}/test/battery_state_broadcaster_params.yaml)
72+
target_include_directories(test_battery_state_broadcaster PRIVATE include)
73+
target_link_libraries(test_battery_state_broadcaster
74+
battery_state_broadcaster
75+
)
76+
endif()
77+
78+
install(
79+
DIRECTORY include/
80+
DESTINATION include/battery_state_broadcaster
81+
)
82+
install(
83+
TARGETS
84+
battery_state_broadcaster
85+
battery_state_broadcaster_parameters
86+
EXPORT export_battery_state_broadcaster
87+
RUNTIME DESTINATION bin
88+
ARCHIVE DESTINATION lib
89+
LIBRARY DESTINATION lib
90+
)
91+
92+
ament_export_targets(export_battery_state_broadcaster HAS_LIBRARY_TARGET)
93+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
94+
ament_package()

‎battery_state_broadcaster/battery_state_broadcaster.xml‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<!--
2+
Copyright (c) 2025, b-robotized Group
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
http://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
16+
-->
17+
18+
<library path="battery_state_broadcaster">
19+
<class name="battery_state_broadcaster/BatteryStateBroadcaster"
20+
type="battery_state_broadcaster::BatteryStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
21+
<description>
22+
This controller publishes the individual battery state of each joint as sensor_msgs/BatteryState messages.
23+
It also publishes the aggregated battery state of all joints as a single control_msgs/BatteryStates message.
24+
</description>
25+
</class>
26+
</library>

‎battery_state_broadcaster/doc/userdoc.rst‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/battery_state_broadcaster/doc/userdoc.rst
2+
3+
.. _battery_state_broadcaster_userdoc:
4+
5+
Battery State Broadcaster
6+
--------------------------------
7+
The *Battery State Broadcaster* is a ROS 2 controller that publishes battery status information as ``sensor_msgs/msg/BatteryState`` messages.
8+
9+
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.
10+
11+
Interfaces
12+
^^^^^^^^^^^
13+
14+
The broadcaster can read the following state interfaces from each configured joint:
15+
16+
- ``battery_voltage`` *(mandatory)* (double)
17+
- ``battery_temperature`` *(optional)* (double)
18+
- ``battery_current`` *(optional)* (double)
19+
- ``battery_charge`` *(optional)* (double)
20+
- ``battery_percentage`` *(optional)* (double)
21+
- ``battery_power_supply_status`` *(optional)* (double)
22+
- ``battery_power_supply_health`` *(optional)* (double)
23+
- ``battery_present`` *(optional)* (bool)
24+
25+
Published Topics
26+
^^^^^^^^^^^^^^^^^^
27+
28+
The broadcaster publishes two topics:
29+
30+
- ``~/raw_battery_states`` (``control_msgs/msg/BatteryStates``)
31+
Publishes **per-joint battery state messages**, containing the raw values for each configured joint.
32+
33+
- ``~/battery_state`` (``sensor_msgs/msg/BatteryState``)
34+
Publishes a **single aggregated battery message** representing the combined status across all joints.
35+
36+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
37+
| Field | ``battery_state`` | ``raw_battery_states`` |
38+
+=============================+======================================================================+=============================================================================================================================================+
39+
| ``header.frame_id`` | Empty | Joint name |
40+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
41+
| ``voltage`` | Mean across all joints | From joint's ``battery_voltage`` interface if enabled, otherwise nan |
42+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
43+
| ``temperature`` | Mean across joints reporting temperature | From joint's ``battery_temperature`` interface if enabled, otherwise nan. |
44+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
45+
| ``current`` | Mean across joints reporting current | From joint's ``battery_current`` interface if enabled, otherwise nan. |
46+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
47+
| ``charge`` | Sum across all joints | From joint's ``battery_charge`` interface if enabled, otherwise nan. |
48+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
49+
| ``capacity`` | Sum across all joints | From joint's ``capacity`` parameter if provided, otherwise nan. |
50+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
51+
| ``design_capacity`` | Sum across all joints | From joint's ``design_capacity`` parameter if provided, otherwise nan. |
52+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
53+
| ``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. |
54+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
55+
| ``power_supply_status`` | Highest reported enum value | From joint's ``battery_power_supply_status`` interface if enabled, otherwise 0 (unknown). |
56+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
57+
| ``power_supply_health`` | Highest reported enum value | From joint's ``battery_power_supply_health`` interface if enabled, otherwise 0 (unknown). |
58+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
59+
| ``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). |
60+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
61+
| ``present`` | True | From joint's ``battery_present`` interface if enabled, otherwise true if joint's voltage values is valid. |
62+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
63+
| ``cell_voltage`` | Empty | Empty |
64+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
65+
| ``cell_temperature`` | Empty | Empty |
66+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
67+
| ``location`` | All joint locations appended | From joint's ``location`` parameter if provided, otherwise empty. |
68+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
69+
| ``serial_number`` | All joint serial numbers appended | From joint's ``serial_number`` parameter if provided, otherwise empty. |
70+
+-----------------------------+----------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------------------+
71+
72+
73+
Parameters
74+
^^^^^^^^^^^
75+
This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to manage parameters.
76+
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.
77+
78+
List of parameters
79+
=========================
80+
.. generate_parameter_library_details:: ../src/battery_state_broadcaster_parameters.yaml
81+
82+
Example Parameter File
83+
=========================
84+
85+
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>`_:
86+
87+
.. literalinclude:: ../test/battery_state_broadcaster_params.yaml
88+
:language: yaml

‎battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
// Copyright (c) 2025, b-robotized Group
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_
16+
#define BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_
17+
18+
#include <cmath>
19+
#include <memory>
20+
#include <string>
21+
#include <unordered_map>
22+
#include <vector>
23+
24+
#include "controller_interface/controller_interface.hpp"
25+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
26+
#include "rclcpp_lifecycle/state.hpp"
27+
#include "realtime_tools/realtime_buffer.hpp"
28+
#include "realtime_tools/realtime_publisher.hpp"
29+
30+
#include <battery_state_broadcaster/battery_state_broadcaster_parameters.hpp>
31+
#include "control_msgs/msg/battery_states.hpp"
32+
#include "sensor_msgs/msg/battery_state.hpp"
33+
34+
namespace battery_state_broadcaster
35+
{
36+
// // name constants for state interfaces
37+
// static constexpr size_t STATE_MY_ITFS = 0;
38+
39+
// // name constants for command interfaces
40+
// static constexpr size_t CMD_MY_ITFS = 0;
41+
42+
/**
43+
* \brief Battery State Broadcaster for all or some state in a ros2_control system.
44+
*
45+
* BatteryStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
46+
* The following state interfaces are published:
47+
* <state_joint>/voltage
48+
* <state_joint>/current
49+
* <state_joint>/charge
50+
* <state_joint>/percentage
51+
* <state_joint>/power_supply_status
52+
* <state_joint>/power_supply_health
53+
* <state_joint>/present
54+
*
55+
* \param state_joints of the batteries to publish.
56+
* \param capacity of the batteries to publish.
57+
* \param design_capacity of the batteries to publish.
58+
* \param power_supply_technology of the batteries to publish.
59+
* \param location of the batteries to publish.
60+
* \param serial_number of the batteries to publish.
61+
*
62+
* Publishes to:
63+
*
64+
* - \b battery_state (sensor_msgs::msg::BatteryState): battery state of the combined battery
65+
* joints.
66+
* - \b raw_battery_states (battery_state_broadcaster::msg::BatteryStates): battery states of the
67+
* individual battery joints.
68+
*
69+
*/
70+
class BatteryStateBroadcaster : public controller_interface::ControllerInterface
71+
{
72+
public:
73+
BatteryStateBroadcaster();
74+
75+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
76+
77+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
78+
79+
controller_interface::CallbackReturn on_init() override;
80+
81+
controller_interface::CallbackReturn on_configure(
82+
const rclcpp_lifecycle::State & previous_state) override;
83+
84+
controller_interface::CallbackReturn on_activate(
85+
const rclcpp_lifecycle::State & previous_state) override;
86+
87+
controller_interface::CallbackReturn on_deactivate(
88+
const rclcpp_lifecycle::State & previous_state) override;
89+
90+
controller_interface::return_type update(
91+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
92+
93+
float get_or_nan(int interface_cnt);
94+
char get_or_unknown(int interface_cnt);
95+
96+
protected:
97+
std::shared_ptr<battery_state_broadcaster::ParamListener> param_listener_;
98+
battery_state_broadcaster::Params params_;
99+
100+
std::vector<std::string> state_joints_;
101+
102+
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::BatteryState>> battery_state_publisher_;
103+
std::shared_ptr<rclcpp::Publisher<control_msgs::msg::BatteryStates>>
104+
raw_battery_states_publisher_;
105+
std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>>
106+
battery_state_realtime_publisher_;
107+
std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::BatteryStates>>
108+
raw_battery_states_realtime_publisher_;
109+
struct BatteryInterfaceSums
110+
{
111+
float voltage_sum = 0.0f;
112+
float temperature_sum = 0.0f;
113+
float current_sum = 0.0f;
114+
float charge_sum = 0.0f;
115+
float percentage_sum = 0.0f;
116+
float capacity_sum = 0.0f;
117+
float design_capacity_sum = 0.0f;
118+
};
119+
120+
struct BatteryInterfaceCounts
121+
{
122+
float temperature_cnt = 0.0f;
123+
float current_cnt = 0.0f;
124+
float percentage_cnt = 0.0f;
125+
};
126+
127+
BatteryInterfaceSums sums_;
128+
BatteryInterfaceCounts counts_;
129+
130+
std::vector<bool> battery_presence_;
131+
132+
private:
133+
};
134+
135+
} // namespace battery_state_broadcaster
136+
137+
#endif // BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_

0 commit comments

Comments
 (0)