From 180de20ade440e269ca7b1ba3ae3b63763faa53b Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 5 Sep 2025 02:36:41 +0300 Subject: [PATCH 01/19] Add battery_state_broadcaster --- battery_state_broadcaster/CMakeLists.txt | 94 ++++ .../battery_state_broadcaster.xml | 26 ++ battery_state_broadcaster/doc/userdoc.rst | 88 ++++ .../battery_state_broadcaster.hpp | 137 ++++++ battery_state_broadcaster/package.xml | 41 ++ .../src/battery_state_broadcaster.cpp | 442 ++++++++++++++++++ .../src/battery_state_broadcaster.yaml | 107 +++++ .../battery_state_broadcaster_params.yaml | 52 +++ .../test/test_battery_state_broadcaster.cpp | 369 +++++++++++++++ .../test/test_battery_state_broadcaster.hpp | 205 ++++++++ .../test_load_battery_state_broadcaster.cpp | 50 ++ ros2_controllers/package.xml | 1 + 12 files changed, 1612 insertions(+) create mode 100644 battery_state_broadcaster/CMakeLists.txt create mode 100644 battery_state_broadcaster/battery_state_broadcaster.xml create mode 100644 battery_state_broadcaster/doc/userdoc.rst create mode 100644 battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp create mode 100644 battery_state_broadcaster/package.xml create mode 100644 battery_state_broadcaster/src/battery_state_broadcaster.cpp create mode 100644 battery_state_broadcaster/src/battery_state_broadcaster.yaml create mode 100644 battery_state_broadcaster/test/battery_state_broadcaster_params.yaml create mode 100644 battery_state_broadcaster/test/test_battery_state_broadcaster.cpp create mode 100644 battery_state_broadcaster/test/test_battery_state_broadcaster.hpp create mode 100644 battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..86b5c263ff --- /dev/null +++ b/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 + $ + $ +) +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() diff --git a/battery_state_broadcaster/battery_state_broadcaster.xml b/battery_state_broadcaster/battery_state_broadcaster.xml new file mode 100644 index 0000000000..11bad6658b --- /dev/null +++ b/battery_state_broadcaster/battery_state_broadcaster.xml @@ -0,0 +1,26 @@ + + + + + + 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. + + + diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0bfde1c6a4 --- /dev/null +++ b/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 `_ to manage parameters. +The parameter `definition file `_ 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 `_: + +.. literalinclude:: ../test/battery_state_broadcaster_params.yaml + :language: yaml diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp new file mode 100644 index 0000000000..3c91e6ffd9 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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 +#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: + * /voltage + * /current + * /charge + * /percentage + * /power_supply_status + * /power_supply_health + * /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 param_listener_; + battery_state_broadcaster::Params params_; + + std::vector state_joints_; + + std::shared_ptr> battery_state_publisher_; + std::shared_ptr> + raw_battery_states_publisher_; + std::shared_ptr> + battery_state_realtime_publisher_; + std::shared_ptr> + 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 battery_presence_; + +private: +}; + +} // namespace battery_state_broadcaster + +#endif // BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_ diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml new file mode 100644 index 0000000000..5975d143ca --- /dev/null +++ b/battery_state_broadcaster/package.xml @@ -0,0 +1,41 @@ + + + + battery_state_broadcaster + 0.1.0 + ros2 control battery state broadcaster controller + Yara Shahin + + Dr. Denis Štogl + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + ament_cmake + + ros2_control_cmake + rosidl_default_generators + sensor_msgs + control_msgs + + rclcpp + controller_interface + + rosidl_default_runtime + sensor_msgs + control_msgs + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp new file mode 100644 index 0000000000..e465151a89 --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -0,0 +1,442 @@ +// 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. + +#include "battery_state_broadcaster/battery_state_broadcaster.hpp" + +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace battery_state_broadcaster +{ +const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); +const size_t MAX_LENGTH = 64; + +BatteryStateBroadcaster::BatteryStateBroadcaster() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn BatteryStateBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during controller's init with message: %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + state_joints_ = params_.state_joints; + battery_presence_.resize(state_joints_.size(), false); + + try + { + battery_state_publisher_ = get_node()->create_publisher( + "~/battery_state", rclcpp::SystemDefaultsQoS()); + + battery_state_realtime_publisher_ = + std::make_shared>( + battery_state_publisher_); + + raw_battery_states_publisher_ = get_node()->create_publisher( + "~/raw_battery_states", rclcpp::SystemDefaultsQoS()); + + raw_battery_states_realtime_publisher_ = + std::make_shared>( + raw_battery_states_publisher_); + } + 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 controller_interface::CallbackReturn::ERROR; + } + + // Reserve memory in state publisher depending on the message type + battery_state_realtime_publisher_->lock(); + battery_state_realtime_publisher_->msg_.location.reserve(MAX_LENGTH); + battery_state_realtime_publisher_->msg_.serial_number.reserve(MAX_LENGTH); + battery_state_realtime_publisher_->unlock(); + + raw_battery_states_realtime_publisher_->lock(); + auto & msg = raw_battery_states_realtime_publisher_->msg_; + msg.battery_states.reserve(state_joints_.size()); + for (size_t i = 0; i < state_joints_.size(); ++i) + { + sensor_msgs::msg::BatteryState battery; + battery.location.reserve(MAX_LENGTH); + battery.serial_number.reserve(MAX_LENGTH); + msg.battery_states.emplace_back(std::move(battery)); + } + raw_battery_states_realtime_publisher_->unlock(); + + // Get count of enabled joints for each interface + for (size_t i = 0; i < state_joints_.size(); ++i) + { + const auto & interfaces = params_.interfaces.state_joints_map.at(params_.state_joints.at(i)); + const auto & battery_properties = params_.state_joints_map.at(params_.state_joints.at(i)); + + if (interfaces.battery_temperature) + { + counts_.temperature_cnt++; + } + if (interfaces.battery_current) + { + counts_.current_cnt++; + } + if (interfaces.battery_percentage) + { + counts_.percentage_cnt++; + } + else + { + auto min_volt = battery_properties.minimum_voltage; + auto max_volt = battery_properties.maximum_voltage; + if ((!std::isnan(min_volt)) && (!std::isnan(max_volt))) + { + if (min_volt >= max_volt) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Maximum battery voltage level must be greater than minimum voltage level."); + return controller_interface::CallbackReturn::ERROR; + } + counts_.percentage_cnt++; + } + } + sums_.capacity_sum += static_cast(battery_properties.capacity); + sums_.design_capacity_sum += static_cast(battery_properties.design_capacity); + } + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +BatteryStateBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration +BatteryStateBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joints_.size() * 8); + for (const auto & joint : state_joints_) + { + const auto & interfaces = params_.interfaces.state_joints_map.at(joint); + state_interfaces_config.names.push_back(joint + "/battery_voltage"); + if (interfaces.battery_temperature) + { + state_interfaces_config.names.push_back(joint + "/battery_temperature"); + } + if (interfaces.battery_current) + { + state_interfaces_config.names.push_back(joint + "/battery_current"); + } + if (interfaces.battery_charge) + { + state_interfaces_config.names.push_back(joint + "/battery_charge"); + } + if (interfaces.battery_percentage) + { + state_interfaces_config.names.push_back(joint + "/battery_percentage"); + } + if (interfaces.battery_power_supply_status) + { + state_interfaces_config.names.push_back(joint + "/battery_power_supply_status"); + } + if (interfaces.battery_power_supply_health) + { + state_interfaces_config.names.push_back(joint + "/battery_power_supply_health"); + } + if (interfaces.battery_present) + { + state_interfaces_config.names.push_back(joint + "/battery_present"); + } + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (state_interfaces_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No state interfaces found to publish."); + return controller_interface::CallbackReturn::FAILURE; + } + + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + auto combined_power_supply_technology = static_cast( + params_.state_joints_map.at(params_.state_joints.at(0)).power_supply_technology); + std::string combined_location = ""; + std::string combined_serial_number = ""; + + // handle individual battery states initializations + auto & raw_battery_states_msg = raw_battery_states_realtime_publisher_->msg_; + for (size_t i = 0; i < state_joints_.size(); ++i) + { + auto & battery_state = raw_battery_states_msg.battery_states[i]; + const auto & battery_properties = params_.state_joints_map.at(params_.state_joints.at(i)); + + battery_state.header.frame_id = state_joints_[i]; + battery_state.voltage = kUninitializedValue; + battery_state.temperature = kUninitializedValue; + battery_state.current = kUninitializedValue; + battery_state.charge = kUninitializedValue; + battery_state.capacity = static_cast(battery_properties.capacity); + battery_state.design_capacity = static_cast(battery_properties.design_capacity); + battery_state.percentage = kUninitializedValue; + battery_state.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + battery_state.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; + battery_state.power_supply_technology = + static_cast(battery_properties.power_supply_technology); + battery_state.present = true; + battery_state.cell_voltage = {}; + battery_state.cell_temperature = {}; + battery_state.location = battery_properties.location; + battery_state.serial_number = battery_properties.serial_number; + + if (combined_power_supply_technology != battery_state.power_supply_technology) + { + combined_power_supply_technology = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; + } + combined_location += battery_state.location + ", "; + combined_serial_number += battery_state.serial_number + ", "; + } + + // handle aggregate battery state initialization + auto & battery_state_msg = battery_state_realtime_publisher_->msg_; + + battery_state_msg.voltage = kUninitializedValue; + battery_state_msg.temperature = kUninitializedValue; + battery_state_msg.current = kUninitializedValue; + battery_state_msg.charge = kUninitializedValue; + battery_state_msg.capacity = sums_.capacity_sum; + battery_state_msg.design_capacity = sums_.design_capacity_sum; + battery_state_msg.percentage = kUninitializedValue; + battery_state_msg.power_supply_status = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + battery_state_msg.power_supply_health = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; + battery_state_msg.power_supply_technology = combined_power_supply_technology; + battery_state_msg.present = true; + battery_state_msg.cell_voltage = {}; + battery_state_msg.cell_temperature = {}; + battery_state_msg.location = combined_location; + battery_state_msg.serial_number = combined_serial_number; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn BatteryStateBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type BatteryStateBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + sums_ = {}; + int interface_cnt = 0; + uint8_t combined_power_supply_status = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + uint8_t combined_power_supply_health = + sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; + + if (raw_battery_states_realtime_publisher_ && raw_battery_states_realtime_publisher_->trylock()) + { + auto & raw_battery_states_msg = raw_battery_states_realtime_publisher_->msg_; + for (size_t i = 0; i < state_joints_.size(); ++i) + { + const auto & interfaces = params_.interfaces.state_joints_map.at(params_.state_joints.at(i)); + + raw_battery_states_msg.battery_states[i].header.stamp = time; + + raw_battery_states_msg.battery_states[i].voltage = get_or_nan(interface_cnt); + sums_.voltage_sum += raw_battery_states_msg.battery_states[i].voltage; + interface_cnt++; + + if (interfaces.battery_temperature) + { + raw_battery_states_msg.battery_states[i].temperature = get_or_nan(interface_cnt); + sums_.temperature_sum += raw_battery_states_msg.battery_states[i].temperature; + interface_cnt++; + } + if (interfaces.battery_current) + { + raw_battery_states_msg.battery_states[i].current = get_or_nan(interface_cnt); + sums_.current_sum += raw_battery_states_msg.battery_states[i].current; + interface_cnt++; + } + if (interfaces.battery_charge) + { + raw_battery_states_msg.battery_states[i].charge = get_or_nan(interface_cnt); + sums_.charge_sum += raw_battery_states_msg.battery_states[i].charge; + interface_cnt++; + } + if (interfaces.battery_percentage) + { + raw_battery_states_msg.battery_states[i].percentage = get_or_nan(interface_cnt); + sums_.percentage_sum += raw_battery_states_msg.battery_states[i].percentage; + interface_cnt++; + } + else + { + auto min_volt = params_.state_joints_map.at(params_.state_joints.at(i)).minimum_voltage; + auto max_volt = params_.state_joints_map.at(params_.state_joints.at(i)).maximum_voltage; + float voltage = raw_battery_states_msg.battery_states[i].voltage; + + raw_battery_states_msg.battery_states[i].percentage = + static_cast((voltage - min_volt) * 100.0 / (max_volt - min_volt)); + sums_.percentage_sum += raw_battery_states_msg.battery_states[i].percentage; + } + if (interfaces.battery_power_supply_status) + { + raw_battery_states_msg.battery_states[i].power_supply_status = + get_or_unknown(interface_cnt); + if ( + raw_battery_states_msg.battery_states[i].power_supply_status > + combined_power_supply_status) + { + combined_power_supply_status = + raw_battery_states_msg.battery_states[i].power_supply_status; + } + interface_cnt++; + } + if (interfaces.battery_power_supply_health) + { + raw_battery_states_msg.battery_states[i].power_supply_health = + get_or_unknown(interface_cnt); + if ( + raw_battery_states_msg.battery_states[i].power_supply_health > + combined_power_supply_health) + { + combined_power_supply_health = + raw_battery_states_msg.battery_states[i].power_supply_health; + } + interface_cnt++; + } + if (interfaces.battery_present) + { + auto opt = state_interfaces_[interface_cnt].get_optional(); + if (opt.has_value()) + { + raw_battery_states_msg.battery_states[i].present = static_cast(*opt); + } + else + { + raw_battery_states_msg.battery_states[i].present = false; + } + interface_cnt++; + } + else + { + if ( + (!std::isnan(raw_battery_states_msg.battery_states[i].voltage)) && + (raw_battery_states_msg.battery_states[i].voltage)) + { + raw_battery_states_msg.battery_states[i].present = true; + } + else + { + raw_battery_states_msg.battery_states[i].present = false; + } + } + } + raw_battery_states_realtime_publisher_->unlockAndPublish(); + } + + if (battery_state_realtime_publisher_ && battery_state_realtime_publisher_->trylock()) + { + auto & battery_state_msg = battery_state_realtime_publisher_->msg_; + + battery_state_msg.header.stamp = time; + battery_state_msg.voltage = sums_.voltage_sum / static_cast(state_joints_.size()); + + if (counts_.temperature_cnt) + { + battery_state_msg.temperature = sums_.temperature_sum / counts_.temperature_cnt; + } + if (counts_.current_cnt) + { + battery_state_msg.current = sums_.current_sum / counts_.current_cnt; + } + battery_state_msg.charge = sums_.charge_sum; + if (counts_.percentage_cnt) + { + battery_state_msg.percentage = sums_.percentage_sum / counts_.percentage_cnt; + } + battery_state_msg.power_supply_status = combined_power_supply_status; + battery_state_msg.power_supply_health = combined_power_supply_health; + + battery_state_realtime_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +float BatteryStateBroadcaster::get_or_nan(int interface_cnt) +{ + auto opt = state_interfaces_[interface_cnt].get_optional(); + if (opt.has_value()) + { + return static_cast(*opt); + } + return std::numeric_limits::quiet_NaN(); +} + +char BatteryStateBroadcaster::get_or_unknown(int interface_cnt) +{ + auto opt = state_interfaces_[interface_cnt].get_optional(); + if (opt.has_value()) + { + return static_cast(*opt); + } + return 0; +} + +} // namespace battery_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + battery_state_broadcaster::BatteryStateBroadcaster, controller_interface::ControllerInterface) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.yaml b/battery_state_broadcaster/src/battery_state_broadcaster.yaml new file mode 100644 index 0000000000..bc4708deec --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster.yaml @@ -0,0 +1,107 @@ +# 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. + +battery_state_broadcaster: + state_joints: { + type: string_array, + description: "List of joints from which battery state interfaces will be read.", + read_only: true, + validation: { + unique<>: null, + size_gt<>: [0] + } + } + interfaces: + __map_state_joints: + battery_temperature: { + type: bool, + default_value: false, + description: "Whether to read battery temperature [°C] from this joint's state interface (If unmeasured NaN)." + } + battery_current: { + type: bool, + default_value: false, + description: "Whether to read battery current [A] from this joint's state interface (If unmeasured NaN)." + } + battery_charge: { + type: bool, + default_value: false, + description: "Whether to read battery charge [Ah] from this joint's state interface (If unmeasured NaN)." + } + battery_percentage: { + type: bool, + default_value: false, + description: "Whether to read charge level [%] (0.0 to 100.0) from this joint's state interface. If unmeasured, linear percentage is calculated using min and max voltage parameters (if provided), otherwise NaN." + } + battery_power_supply_status: { + type: bool, + default_value: false, + description: "Whether to read power supply status (e.g., Charging, Full) from this joint's state interface. If unmeasured, status is set to unknown." + } + battery_power_supply_health: { + type: bool, + default_value: false, + description: "Whether to read power supply health (e.g., Good, Overheat) from this joint's state interface. If unmeasured, health is set to unknown." + } + battery_present: { + type: bool, + default_value: false, + description: "Whether to read battery presence status (true if battery is present) from this joint's state interface. If unmeasured, the presence will be set to true if any other state interfaces from this joint are available." + } + __map_state_joints: + minimum_voltage: { + type: double, + default_value: .nan, + description: "Minimum battery voltage (used to calculate percentage).", + read_only: true, + } + maximum_voltage: { + type: double, + default_value: .nan, + description: "Maximum battery voltage (used to calculate percentage).", + read_only: true, + } + capacity: { + type: double, + default_value: .nan, + description: "Last known full battery capacity [Ah] (If unmeasured NaN).", + read_only: true, + } + design_capacity: { + type: double, + default_value: .nan, + description: "Design capacity of the battery [Ah] (If unmeasured NaN).", + read_only: true, + } + power_supply_technology: { + type: int, + default_value: 0, + description: "Battery chemistry type as an enum. If unmeasured, the technology is set to unknown.", + read_only: true, + validation: { + bounds<>: [0, 8] + } + } + location: { + type: string, + default_value: "", + description: "Physical location of the battery (e.g., slot number or plug label).", + read_only: true, + } + serial_number: { + type: string, + default_value: "", + description: "Serial number of the battery.", + read_only: true, + } diff --git a/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml b/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml new file mode 100644 index 0000000000..2efa5fc8f2 --- /dev/null +++ b/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml @@ -0,0 +1,52 @@ +# 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. + +test_battery_state_broadcaster: + ros__parameters: + state_joints: + - "left_wheel" + - "right_wheel" + interfaces: + left_wheel: + battery_temperature: true + battery_current: false + battery_charge: true + battery_percentage: false + battery_power_supply_status: true + battery_power_supply_health: true + battery_present: false + right_wheel: + battery_temperature: true + battery_current: true + battery_charge: true + battery_percentage: true + battery_power_supply_status: true + battery_power_supply_health: true + battery_present: false + left_wheel: + minimum_voltage: 0.0 + maximum_voltage: 10.0 + capacity: 12000.0 + design_capacity: 13000.0 + power_supply_technology: 3 + location: "left_slot" + serial_number: "left_serial_device" + right_wheel: + minimum_voltage: 0.0 + maximum_voltage: 15.0 + capacity: 17000.0 + design_capacity: 18000.0 + power_supply_technology: 3 + location: "right_slot" + serial_number: "right_serial_device" diff --git a/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp b/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp new file mode 100644 index 0000000000..09780ccd4b --- /dev/null +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp @@ -0,0 +1,369 @@ +// 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. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_battery_state_broadcaster.hpp" + +// Test correct broadcaster initialization +TEST_F(BatteryStateBroadcasterTest, init_success) { SetUpBatteryStateBroadcaster(); } + +// Test that BatteryStateBroadcaster parses parameters correctly, +// Test that BatteryStateBroadcaster aggregates interfaces and battery properties correctly, +// sets up state interfaces on configure, and computes aggregated counts/sums. +TEST_F(BatteryStateBroadcasterTest, all_parameters_set_configure_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(battery_state_broadcaster_->params_.state_joints.empty()); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + battery_state_broadcaster_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + + auto interface_params = battery_state_broadcaster_->params_.interfaces.state_joints_map; + auto properties = battery_state_broadcaster_->params_.state_joints_map; + EXPECT_EQ(interface_params.at("left_wheel").battery_temperature, true); + EXPECT_EQ(interface_params.at("left_wheel").battery_current, false); + EXPECT_EQ(interface_params.at("left_wheel").battery_charge, true); + EXPECT_EQ(interface_params.at("left_wheel").battery_percentage, false); + EXPECT_EQ(interface_params.at("left_wheel").battery_power_supply_status, true); + EXPECT_EQ(interface_params.at("left_wheel").battery_power_supply_health, true); + EXPECT_EQ(interface_params.at("left_wheel").battery_present, false); + + EXPECT_EQ(interface_params.at("right_wheel").battery_temperature, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_current, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_charge, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_percentage, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_power_supply_status, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_power_supply_health, true); + EXPECT_EQ(interface_params.at("right_wheel").battery_present, false); + + EXPECT_EQ(properties.at("left_wheel").minimum_voltage, 0.0); + EXPECT_EQ(properties.at("left_wheel").maximum_voltage, 10.0); + EXPECT_EQ(properties.at("left_wheel").capacity, 12000.0); + EXPECT_EQ(properties.at("left_wheel").design_capacity, 13000.0); + EXPECT_EQ(properties.at("left_wheel").power_supply_technology, 3); + EXPECT_EQ(properties.at("left_wheel").location, "left_slot"); + EXPECT_EQ(properties.at("left_wheel").serial_number, "left_serial_device"); + + EXPECT_EQ(properties.at("right_wheel").minimum_voltage, 0.0); + EXPECT_EQ(properties.at("right_wheel").maximum_voltage, 15.0); + EXPECT_EQ(properties.at("right_wheel").capacity, 17000.0); + EXPECT_EQ(properties.at("right_wheel").design_capacity, 18000.0); + EXPECT_EQ(properties.at("right_wheel").power_supply_technology, 3); + EXPECT_EQ(properties.at("right_wheel").location, "right_slot"); + EXPECT_EQ(properties.at("right_wheel").serial_number, "right_serial_device"); + + // check property aggregation + EXPECT_EQ(battery_state_broadcaster_->counts_.temperature_cnt, 2.0); + EXPECT_EQ(battery_state_broadcaster_->counts_.current_cnt, 1.0); + EXPECT_EQ( + battery_state_broadcaster_->counts_.percentage_cnt, + 2.0); // because min and max voltage are valid + EXPECT_EQ(battery_state_broadcaster_->sums_.capacity_sum, 29000.0); + EXPECT_EQ(battery_state_broadcaster_->sums_.design_capacity_sum, 31000.0); + + // check interface configuration + auto cmd_if_conf = battery_state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = battery_state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(12lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +// check fails when no defined interfaces +TEST_F(BatteryStateBroadcasterTest, no_interfaces_set_activate_fail) +{ + ASSERT_EQ( + battery_state_broadcaster_->init( + "test_battery_state_broadcaster", "", 0, "", + battery_state_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +// check all msgs initial values +// check logic for combined strings for local & serial number and same or none for power supply +TEST_F(BatteryStateBroadcasterTest, activate_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + RawBatteryStatesMsg raw_battery_states_msg = + battery_state_broadcaster_->raw_battery_states_realtime_publisher_->msg_; + EXPECT_EQ(raw_battery_states_msg.battery_states.size(), static_cast(2)); + + // --- Left wheel --- + const auto & left = raw_battery_states_msg.battery_states[0]; + EXPECT_EQ(left.header.frame_id, "left_wheel"); + EXPECT_TRUE(std::isnan(left.voltage)); + EXPECT_TRUE(std::isnan(left.temperature)); + EXPECT_TRUE(std::isnan(left.current)); + EXPECT_TRUE(std::isnan(left.charge)); + EXPECT_FLOAT_EQ(left.capacity, 12000.0f); + EXPECT_FLOAT_EQ(left.design_capacity, 13000.0f); + EXPECT_TRUE(std::isnan(left.percentage)); + EXPECT_EQ(left.power_supply_status, BatteryState::POWER_SUPPLY_STATUS_UNKNOWN); + EXPECT_EQ(left.power_supply_health, BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN); + EXPECT_EQ(left.power_supply_technology, 3); + EXPECT_TRUE(left.present); + EXPECT_TRUE(left.cell_voltage.empty()); + EXPECT_TRUE(left.cell_temperature.empty()); + EXPECT_EQ(left.location, "left_slot"); + EXPECT_EQ(left.serial_number, "left_serial_device"); + + // --- Right wheel --- + const auto & right = raw_battery_states_msg.battery_states[1]; + EXPECT_EQ(right.header.frame_id, "right_wheel"); + EXPECT_TRUE(std::isnan(right.voltage)); + EXPECT_TRUE(std::isnan(right.temperature)); + EXPECT_TRUE(std::isnan(right.current)); + EXPECT_TRUE(std::isnan(right.charge)); + EXPECT_FLOAT_EQ(right.capacity, 17000.0f); + EXPECT_FLOAT_EQ(right.design_capacity, 18000.0f); + EXPECT_TRUE(std::isnan(right.percentage)); + EXPECT_EQ(right.power_supply_status, BatteryState::POWER_SUPPLY_STATUS_UNKNOWN); + EXPECT_EQ(right.power_supply_health, BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN); + EXPECT_EQ(right.power_supply_technology, 3); + EXPECT_TRUE(right.present); + EXPECT_TRUE(right.cell_voltage.empty()); + EXPECT_TRUE(right.cell_temperature.empty()); + EXPECT_EQ(right.location, "right_slot"); + EXPECT_EQ(right.serial_number, "right_serial_device"); + + BatteryStateMsg battery_state_msg = + battery_state_broadcaster_->battery_state_realtime_publisher_->msg_; + + EXPECT_TRUE(std::isnan(battery_state_msg.voltage)); + EXPECT_TRUE(std::isnan(battery_state_msg.temperature)); + EXPECT_TRUE(std::isnan(battery_state_msg.current)); + EXPECT_TRUE(std::isnan(battery_state_msg.charge)); + EXPECT_TRUE(std::isnan(battery_state_msg.percentage)); + EXPECT_DOUBLE_EQ(battery_state_msg.capacity, 29000.0); + EXPECT_DOUBLE_EQ(battery_state_msg.design_capacity, 31000.0); + EXPECT_EQ(battery_state_msg.power_supply_status, BatteryState::POWER_SUPPLY_STATUS_UNKNOWN); + EXPECT_EQ(battery_state_msg.power_supply_health, BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN); + EXPECT_EQ(battery_state_msg.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_EQ(battery_state_msg.location, "left_slot, right_slot, "); + EXPECT_EQ(battery_state_msg.serial_number, "left_serial_device, right_serial_device, "); + EXPECT_TRUE(battery_state_msg.present); + EXPECT_TRUE(battery_state_msg.cell_voltage.empty()); + EXPECT_TRUE(battery_state_msg.cell_temperature.empty()); +} + +TEST_F(BatteryStateBroadcasterTest, deactivate_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(BatteryStateBroadcasterTest, check_exported_intefaces) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = battery_state_broadcaster_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), static_cast(0)); + + auto state_interfaces = battery_state_broadcaster_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), itfs_values_.size()); + EXPECT_EQ(state_interfaces.names[0], "left_wheel/battery_voltage"); + EXPECT_EQ(state_interfaces.names[1], "left_wheel/battery_temperature"); + EXPECT_EQ(state_interfaces.names[2], "left_wheel/battery_charge"); + EXPECT_EQ(state_interfaces.names[3], "left_wheel/battery_power_supply_status"); + EXPECT_EQ(state_interfaces.names[4], "left_wheel/battery_power_supply_health"); + EXPECT_EQ(state_interfaces.names[5], "right_wheel/battery_voltage"); + EXPECT_EQ(state_interfaces.names[6], "right_wheel/battery_temperature"); + EXPECT_EQ(state_interfaces.names[7], "right_wheel/battery_current"); + EXPECT_EQ(state_interfaces.names[8], "right_wheel/battery_charge"); + EXPECT_EQ(state_interfaces.names[9], "right_wheel/battery_percentage"); + EXPECT_EQ(state_interfaces.names[10], "right_wheel/battery_power_supply_status"); + EXPECT_EQ(state_interfaces.names[11], "right_wheel/battery_power_supply_health"); +} + +TEST_F(BatteryStateBroadcasterTest, update_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + battery_state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// check correct values published +// check correct aggregation for present, percentage, averages, and sums, and higher criticality +TEST_F(BatteryStateBroadcasterTest, publish_status_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + battery_state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + RawBatteryStatesMsg raw_battery_states_msg; + BatteryStateMsg battery_state_msg; + subscribe_and_get_messages(raw_battery_states_msg, battery_state_msg); + + ASSERT_EQ(raw_battery_states_msg.battery_states.size(), 2u); + + // Left wheel + const auto & left = raw_battery_states_msg.battery_states[0]; + EXPECT_EQ(left.header.frame_id, "left_wheel"); + EXPECT_DOUBLE_EQ(left.voltage, 5.0); + EXPECT_DOUBLE_EQ(left.temperature, 60.0); + EXPECT_TRUE(std::isnan(left.current)); // disabled in params + EXPECT_DOUBLE_EQ(left.charge, 6000.0); + EXPECT_DOUBLE_EQ(left.capacity, 12000.0); + EXPECT_DOUBLE_EQ(left.design_capacity, 13000.0); + // percentage calculated (no interface) = (5.0 - 0.0) * 100 / (10.0 - 0.0) = 50 + EXPECT_DOUBLE_EQ(left.percentage, 50.0); + EXPECT_EQ(left.power_supply_status, 3); // from itfs_values_[3] + EXPECT_EQ(left.power_supply_health, 0); // from itfs_values_[4] + EXPECT_EQ(left.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_TRUE(left.present); // voltage > 0.0 + EXPECT_EQ(left.location, "left_slot"); + EXPECT_EQ(left.serial_number, "left_serial_device"); + + // Right wheel + const auto & right = raw_battery_states_msg.battery_states[1]; + EXPECT_EQ(right.header.frame_id, "right_wheel"); + EXPECT_DOUBLE_EQ(right.voltage, 10.0); + EXPECT_DOUBLE_EQ(right.temperature, 80.0); + EXPECT_DOUBLE_EQ(right.current, 2000.0); + EXPECT_DOUBLE_EQ(right.charge, 5000.0); + EXPECT_DOUBLE_EQ(right.capacity, 17000.0); + EXPECT_DOUBLE_EQ(right.design_capacity, 18000.0); + EXPECT_DOUBLE_EQ(right.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_EQ(right.power_supply_status, 2); // from itfs_values_[10] + EXPECT_EQ(right.power_supply_health, 4); // from itfs_values_[11] + EXPECT_EQ(right.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_TRUE(right.present); // voltage > 0.0 + EXPECT_EQ(right.location, "right_slot"); + EXPECT_EQ(right.serial_number, "right_serial_device"); + + // Combined battery state message + EXPECT_EQ(battery_state_msg.header.frame_id, ""); + EXPECT_DOUBLE_EQ(battery_state_msg.voltage, 7.5); // average of 5 + 10 + EXPECT_DOUBLE_EQ(battery_state_msg.temperature, 70.0); // average of 60 + 80 + EXPECT_DOUBLE_EQ(battery_state_msg.current, 2000.0); // only right wheel contributes + EXPECT_DOUBLE_EQ(battery_state_msg.charge, 11000.0); // sum of 6000 + 5000 + EXPECT_DOUBLE_EQ(battery_state_msg.capacity, 29000.0); // sum of 6000 + 5000 + EXPECT_DOUBLE_EQ(battery_state_msg.design_capacity, 31000.0); // sum of 6000 + 5000 + EXPECT_DOUBLE_EQ(battery_state_msg.percentage, 58.0); // average of 50 + 66 + EXPECT_EQ(battery_state_msg.power_supply_status, 3); // max(3, 2) + EXPECT_EQ(battery_state_msg.power_supply_health, 4); // max(0, 4) + EXPECT_EQ(battery_state_msg.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_TRUE(battery_state_msg.present); // voltage > 0.0 + EXPECT_EQ(battery_state_msg.location, "left_slot, right_slot, "); + EXPECT_EQ(battery_state_msg.serial_number, "left_serial_device, right_serial_device, "); +} + +TEST_F(BatteryStateBroadcasterTest, update_broadcasted_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(left_voltage_itf_.set_value(10.0)); + + RawBatteryStatesMsg raw_battery_states_msg; + BatteryStateMsg battery_state_msg; + subscribe_and_get_messages(raw_battery_states_msg, battery_state_msg); + + ASSERT_EQ(raw_battery_states_msg.battery_states.size(), 2u); + + // Left wheel + const auto & left = raw_battery_states_msg.battery_states[0]; + EXPECT_DOUBLE_EQ(left.voltage, 10.0); + // percentage calculated (no interface) = (10.0 - 0.0) * 100 / (10.0 - 0.0) = 50 + EXPECT_DOUBLE_EQ(left.percentage, 100.0); + EXPECT_TRUE(left.present); // voltage > 0.0 + + // Right wheel + const auto & right = raw_battery_states_msg.battery_states[1]; + EXPECT_DOUBLE_EQ(right.voltage, 10.0); + EXPECT_DOUBLE_EQ(right.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_TRUE(right.present); // voltage > 0.0 + + // Combined battery state message + EXPECT_DOUBLE_EQ(battery_state_msg.voltage, 10.0); // average of 10 + 10 + EXPECT_DOUBLE_EQ(battery_state_msg.percentage, 83.0); // average of 100 + 66 + EXPECT_TRUE(battery_state_msg.present); // voltage > 0.0 +} + +TEST_F(BatteryStateBroadcasterTest, publish_nan_voltage) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_EQ(battery_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(battery_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(left_voltage_itf_.set_value(std::numeric_limits::quiet_NaN())); + + RawBatteryStatesMsg raw_battery_states_msg; + BatteryStateMsg battery_state_msg; + subscribe_and_get_messages(raw_battery_states_msg, battery_state_msg); + + ASSERT_EQ(raw_battery_states_msg.battery_states.size(), 2u); + + // Left wheel + const auto & left = raw_battery_states_msg.battery_states[0]; + EXPECT_TRUE(std::isnan(left.voltage)); + EXPECT_TRUE(std::isnan(left.percentage)); + EXPECT_FALSE(left.present); // voltage nan + + // Right wheel + const auto & right = raw_battery_states_msg.battery_states[1]; + EXPECT_DOUBLE_EQ(right.voltage, 10.0); + EXPECT_DOUBLE_EQ(right.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_TRUE(right.present); // voltage > 0.0 + + // Combined battery state message + EXPECT_TRUE(std::isnan(battery_state_msg.voltage)); // average of nan + 10 + EXPECT_TRUE(std::isnan(battery_state_msg.percentage)); // average of nan + 66 + EXPECT_TRUE(battery_state_msg.present); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp new file mode 100644 index 0000000000..a3fd271935 --- /dev/null +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp @@ -0,0 +1,205 @@ +// 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 TEST_BATTERY_STATE_BROADCASTER_HPP_ +#define TEST_BATTERY_STATE_BROADCASTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "battery_state_broadcaster/battery_state_broadcaster.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "control_msgs/msg/battery_states.hpp" +#include "sensor_msgs/msg/battery_state.hpp" + +using BatteryStateMsg = sensor_msgs::msg::BatteryState; +using RawBatteryStatesMsg = control_msgs::msg::BatteryStates; +using sensor_msgs::msg::BatteryState; +using testing::IsEmpty; +using testing::SizeIs; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +constexpr auto NODE_FAILURE = controller_interface::CallbackReturn::FAILURE; +} // namespace + +// subclassing and friending so we can access member variables +class FriendBatteryStateBroadcaster : public battery_state_broadcaster::BatteryStateBroadcaster +{ + FRIEND_TEST(BatteryStateBroadcasterTest, init_success); + FRIEND_TEST(BatteryStateBroadcasterTest, all_parameters_set_configure_success); + FRIEND_TEST(BatteryStateBroadcasterTest, no_interfaces_set_activate_fail); + FRIEND_TEST(BatteryStateBroadcasterTest, activate_success); + FRIEND_TEST(BatteryStateBroadcasterTest, deactivate_success); + FRIEND_TEST(BatteryStateBroadcasterTest, check_exported_intefaces); + FRIEND_TEST(BatteryStateBroadcasterTest, update_success); + FRIEND_TEST(BatteryStateBroadcasterTest, publish_status_success); + FRIEND_TEST(BatteryStateBroadcasterTest, update_broadcasted_success); + FRIEND_TEST(BatteryStateBroadcasterTest, publish_nan_voltage); +}; + +class BatteryStateBroadcasterTest : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + static void TearDownTestCase() {} + + void SetUp() + { + // initialize controller + battery_state_broadcaster_ = std::make_unique(); + } + void TearDown() { battery_state_broadcaster_.reset(nullptr); } + + void SetUpBatteryStateBroadcaster( + const std::string controller_name = "test_battery_state_broadcaster") + { + ASSERT_EQ( + battery_state_broadcaster_->init( + controller_name, "", 0, "", battery_state_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_ifs; + + state_ifs.emplace_back(left_voltage_itf_); + state_ifs.emplace_back(left_temperature_itf_); + state_ifs.emplace_back(left_charge_itf_); + state_ifs.emplace_back(left_status_itf_); + state_ifs.emplace_back(left_health_itf_); + + state_ifs.emplace_back(right_voltage_itf_); + state_ifs.emplace_back(right_temperature_itf_); + state_ifs.emplace_back(right_current_itf_); + state_ifs.emplace_back(right_charge_itf_); + state_ifs.emplace_back(right_percentage_itf_); + state_ifs.emplace_back(right_status_itf_); + state_ifs.emplace_back(right_health_itf_); + + battery_state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); + } + +protected: + // Controller-related parameters + std::vector state_joint_names_ = {"left_wheel", "right_wheel"}; + std::array itfs_values_ = { + 5.0, // 0 left_voltage + 60.0, // 1 left_temperature + 6000.0, // 2 left_charge + 3.0, // 3 left_status + 0.0, // 4 left_health + 10.0, // 5 right_voltage + 80.0, // 6 right_temperature + 2000.0, // 7 right_current + 5000.0, // 8 right_charge + 66.0, // 9 right_percentage + 2.0, // 10 right_status + 4.0 // 11 right_health + }; + // std::array itfs_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; + + hardware_interface::StateInterface left_voltage_itf_{ + "left_wheel", "battery_voltage", &itfs_values_[0]}; + hardware_interface::StateInterface left_temperature_itf_{ + "left_wheel", "battery_temperature", &itfs_values_[1]}; + hardware_interface::StateInterface left_charge_itf_{ + "left_wheel", "battery_charge", &itfs_values_[2]}; + hardware_interface::StateInterface left_status_itf_{ + "left_wheel", "battery_power_supply_status", &itfs_values_[3]}; + hardware_interface::StateInterface left_health_itf_{ + "left_wheel", "battery_power_supply_health", &itfs_values_[4]}; + hardware_interface::StateInterface right_voltage_itf_{ + "right_wheel", "battery_voltage", &itfs_values_[5]}; + hardware_interface::StateInterface right_temperature_itf_{ + "right_wheel", "battery_temperature", &itfs_values_[6]}; + hardware_interface::StateInterface right_current_itf_{ + "right_wheel", "battery_current", &itfs_values_[7]}; + hardware_interface::StateInterface right_charge_itf_{ + "right_wheel", "battery_charge", &itfs_values_[8]}; + hardware_interface::StateInterface right_percentage_itf_{ + "right_wheel", "battery_percentage", &itfs_values_[9]}; + hardware_interface::StateInterface right_status_itf_{ + "right_wheel", "battery_power_supply_status", &itfs_values_[10]}; + hardware_interface::StateInterface right_health_itf_{ + "right_wheel", "battery_power_supply_health", &itfs_values_[11]}; + + // Test related parameters + std::unique_ptr battery_state_broadcaster_; + + void subscribe_and_get_messages( + RawBatteryStatesMsg & raw_battery_states_msg, BatteryStateMsg & battery_state_msg) + { + // create a new subscriber + RawBatteryStatesMsg::SharedPtr received_raw_battery_states_msg; + BatteryStateMsg::SharedPtr received_battery_state_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto raw_battery_states_callback = [&](const RawBatteryStatesMsg::SharedPtr msg) + { received_raw_battery_states_msg = msg; }; + auto battery_state_callback = [&](const BatteryStateMsg::SharedPtr msg) + { received_battery_state_msg = msg; }; + auto raw_battery_states_subscription = + test_subscription_node.create_subscription( + "/test_battery_state_broadcaster/raw_battery_states", 10, raw_battery_states_callback); + auto battery_state_subscription = test_subscription_node.create_subscription( + "/test_battery_state_broadcaster/battery_state", 10, battery_state_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + battery_state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while ((!received_battery_state_msg || !received_raw_battery_states_msg) && + test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_raw_battery_states_msg.get() && received_battery_state_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_raw_battery_states_msg); + ASSERT_TRUE(received_battery_state_msg); + + // take message from subscription + raw_battery_states_msg = *received_raw_battery_states_msg; + battery_state_msg = *received_battery_state_msg; + } +}; + +#endif // TEST_BATTERY_STATE_BROADCASTER_HPP_ diff --git a/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp b/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp new file mode 100644 index 0000000000..0fe90d309c --- /dev/null +++ b/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp @@ -0,0 +1,50 @@ +// 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadBatteryStateBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/battery_state_broadcaster_params.yaml"; + + cm.set_parameter({"test_battery_state_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_battery_state_broadcaster.type", "battery_state_broadcaster/BatteryStateBroadcaster"}); + + ASSERT_NO_THROW(cm.load_controller( + "test_battery_state_broadcaster", "battery_state_broadcaster/BatteryStateBroadcaster")); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b41c116658..24c4316685 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -19,6 +19,7 @@ ackermann_steering_controller admittance_controller + battery_state_broadcaster bicycle_steering_controller chained_filter_controller diff_drive_controller From c2da6a3359e8ebffeb46d676228901321b2d173c Mon Sep 17 00:00:00 2001 From: Yara Shahin <58101871+YaraShahin@users.noreply.github.com> Date: Wed, 1 Oct 2025 02:27:42 +0300 Subject: [PATCH 02/19] Update battery_state_broadcaster/doc/userdoc.rst Co-authored-by: Bence Magyar --- battery_state_broadcaster/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst index 0bfde1c6a4..8ddf46810e 100644 --- a/battery_state_broadcaster/doc/userdoc.rst +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -4,7 +4,7 @@ Battery State Broadcaster -------------------------------- -The *Battery State Broadcaster* is a ROS 2 controller that publishes battery status information as ``sensor_msgs/msg/BatteryState`` messages. +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. From f1f5b293dd42aff9b46ea8ec4c56864f8a91c1f3 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 1 Oct 2025 02:33:44 +0300 Subject: [PATCH 03/19] edit maintainers and author order --- battery_state_broadcaster/package.xml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml index 5975d143ca..1563046e88 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -4,9 +4,11 @@ battery_state_broadcaster 0.1.0 ros2 control battery state broadcaster controller - Yara Shahin - Dr. Denis Štogl + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 @@ -14,6 +16,8 @@ https://github.com/ros-controls/ros2_controllers/issues https://github.com/ros-controls/ros2_controllers/ + Yara Shahin + ament_cmake ros2_control_cmake From f71f294769852f70ab2bc26795f3378d735a3f6a Mon Sep 17 00:00:00 2001 From: Yara Shahin <58101871+YaraShahin@users.noreply.github.com> Date: Wed, 1 Oct 2025 02:35:08 +0300 Subject: [PATCH 04/19] Update battery_state_broadcaster/package.xml Co-authored-by: Bence Magyar --- battery_state_broadcaster/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml index 1563046e88..6358ba2269 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -33,8 +33,6 @@ control_msgs ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager hardware_interface_testing ros2_control_test_assets From 3e12bc1127da62e4a3df535e84efec32b83253c1 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 1 Oct 2025 02:49:32 +0300 Subject: [PATCH 05/19] edit package dependencies --- battery_state_broadcaster/package.xml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml index 6358ba2269..aef917c42a 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -22,15 +22,13 @@ ros2_control_cmake rosidl_default_generators - sensor_msgs - control_msgs rclcpp controller_interface + sensor_msgs + control_msgs rosidl_default_runtime - sensor_msgs - control_msgs ament_cmake_gmock controller_manager From d6626d96ae4adc3b3b79317a7969fea0d6ecbd6e Mon Sep 17 00:00:00 2001 From: Yara Shahin <58101871+YaraShahin@users.noreply.github.com> Date: Wed, 1 Oct 2025 02:50:56 +0300 Subject: [PATCH 06/19] Update battery_state_broadcaster/src/battery_state_broadcaster.cpp Co-authored-by: Bence Magyar --- battery_state_broadcaster/src/battery_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index e465151a89..cb5fd08792 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -25,7 +25,7 @@ namespace battery_state_broadcaster { const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); -const size_t MAX_LENGTH = 64; +const size_t MAX_LENGTH = 64; // maximum length of strings to reserve BatteryStateBroadcaster::BatteryStateBroadcaster() : controller_interface::ControllerInterface() {} From deca511a57f648614206c38325a51f3baa730fcf Mon Sep 17 00:00:00 2001 From: Yara Shahin <58101871+YaraShahin@users.noreply.github.com> Date: Wed, 1 Oct 2025 02:51:11 +0300 Subject: [PATCH 07/19] Update battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp Co-authored-by: Bence Magyar --- .../battery_state_broadcaster/battery_state_broadcaster.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index 3c91e6ffd9..606b9ac92d 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -33,12 +33,6 @@ 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. * From bc3fadc9b2bbbe51052bdafaf00f9b56db6cf0aa Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 1 Oct 2025 04:24:16 +0300 Subject: [PATCH 08/19] fix interface types --- .../battery_state_broadcaster.hpp | 4 +- .../src/battery_state_broadcaster.cpp | 45 +++++++------------ 2 files changed, 17 insertions(+), 32 deletions(-) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index 606b9ac92d..a8fdefa402 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -16,6 +16,7 @@ #define BATTERY_STATE_BROADCASTER__BATTERY_STATE_BROADCASTER_HPP_ #include +#include #include #include #include @@ -84,9 +85,6 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface 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 param_listener_; battery_state_broadcaster::Params params_; diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index cb5fd08792..fe1b11bedf 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -203,7 +203,7 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - auto combined_power_supply_technology = static_cast( + auto combined_power_supply_technology = static_cast( params_.state_joints_map.at(params_.state_joints.at(0)).power_supply_technology); std::string combined_location = ""; std::string combined_serial_number = ""; @@ -226,7 +226,7 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( battery_state.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; battery_state.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; battery_state.power_supply_technology = - static_cast(battery_properties.power_supply_technology); + static_cast(battery_properties.power_supply_technology); battery_state.present = true; battery_state.cell_voltage = {}; battery_state.cell_temperature = {}; @@ -291,31 +291,36 @@ controller_interface::return_type BatteryStateBroadcaster::update( raw_battery_states_msg.battery_states[i].header.stamp = time; - raw_battery_states_msg.battery_states[i].voltage = get_or_nan(interface_cnt); + raw_battery_states_msg.battery_states[i].voltage = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); sums_.voltage_sum += raw_battery_states_msg.battery_states[i].voltage; interface_cnt++; if (interfaces.battery_temperature) { - raw_battery_states_msg.battery_states[i].temperature = get_or_nan(interface_cnt); + raw_battery_states_msg.battery_states[i].temperature = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); sums_.temperature_sum += raw_battery_states_msg.battery_states[i].temperature; interface_cnt++; } if (interfaces.battery_current) { - raw_battery_states_msg.battery_states[i].current = get_or_nan(interface_cnt); + raw_battery_states_msg.battery_states[i].current = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); sums_.current_sum += raw_battery_states_msg.battery_states[i].current; interface_cnt++; } if (interfaces.battery_charge) { - raw_battery_states_msg.battery_states[i].charge = get_or_nan(interface_cnt); + raw_battery_states_msg.battery_states[i].charge = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); sums_.charge_sum += raw_battery_states_msg.battery_states[i].charge; interface_cnt++; } if (interfaces.battery_percentage) { - raw_battery_states_msg.battery_states[i].percentage = get_or_nan(interface_cnt); + raw_battery_states_msg.battery_states[i].percentage = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); sums_.percentage_sum += raw_battery_states_msg.battery_states[i].percentage; interface_cnt++; } @@ -332,7 +337,8 @@ controller_interface::return_type BatteryStateBroadcaster::update( if (interfaces.battery_power_supply_status) { raw_battery_states_msg.battery_states[i].power_supply_status = - get_or_unknown(interface_cnt); + static_cast(state_interfaces_[interface_cnt].get_optional().value_or( + sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN)); if ( raw_battery_states_msg.battery_states[i].power_supply_status > combined_power_supply_status) @@ -345,7 +351,8 @@ controller_interface::return_type BatteryStateBroadcaster::update( if (interfaces.battery_power_supply_health) { raw_battery_states_msg.battery_states[i].power_supply_health = - get_or_unknown(interface_cnt); + static_cast(state_interfaces_[interface_cnt].get_optional().value_or( + sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN)); if ( raw_battery_states_msg.battery_states[i].power_supply_health > combined_power_supply_health) @@ -414,26 +421,6 @@ controller_interface::return_type BatteryStateBroadcaster::update( return controller_interface::return_type::OK; } -float BatteryStateBroadcaster::get_or_nan(int interface_cnt) -{ - auto opt = state_interfaces_[interface_cnt].get_optional(); - if (opt.has_value()) - { - return static_cast(*opt); - } - return std::numeric_limits::quiet_NaN(); -} - -char BatteryStateBroadcaster::get_or_unknown(int interface_cnt) -{ - auto opt = state_interfaces_[interface_cnt].get_optional(); - if (opt.has_value()) - { - return static_cast(*opt); - } - return 0; -} - } // namespace battery_state_broadcaster #include "pluginlib/class_list_macros.hpp" From 8afbf1b33c274a3ee7df29fd106c936bae533b51 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 1 Oct 2025 05:02:51 +0300 Subject: [PATCH 09/19] move protected members to private context --- .../battery_state_broadcaster.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index a8fdefa402..372a45d6f3 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -86,14 +86,10 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::shared_ptr param_listener_; battery_state_broadcaster::Params params_; std::vector state_joints_; - std::shared_ptr> battery_state_publisher_; - std::shared_ptr> - raw_battery_states_publisher_; std::shared_ptr> battery_state_realtime_publisher_; std::shared_ptr> @@ -119,9 +115,13 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface BatteryInterfaceSums sums_; BatteryInterfaceCounts counts_; - std::vector battery_presence_; - private: + std::shared_ptr param_listener_; + std::shared_ptr> battery_state_publisher_; + std::shared_ptr> + raw_battery_states_publisher_; + + std::vector battery_presence_; }; } // namespace battery_state_broadcaster From 357c6320ae4bab8f62f9639b28131cb87e5cf32b Mon Sep 17 00:00:00 2001 From: Yara Shahin <58101871+YaraShahin@users.noreply.github.com> Date: Wed, 1 Oct 2025 05:06:19 +0300 Subject: [PATCH 10/19] Update battery_state_broadcaster/src/battery_state_broadcaster.cpp Co-authored-by: Bence Magyar --- battery_state_broadcaster/src/battery_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index fe1b11bedf..8527008551 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -367,7 +367,7 @@ controller_interface::return_type BatteryStateBroadcaster::update( auto opt = state_interfaces_[interface_cnt].get_optional(); if (opt.has_value()) { - raw_battery_states_msg.battery_states[i].present = static_cast(*opt); + raw_battery_states_msg.battery_states[i].present = opt->value(); } else { From db48991ae0d29662caa27cfb68b93350e1343146 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 1 Oct 2025 05:52:40 +0300 Subject: [PATCH 11/19] remove bool from get optional --- .../src/battery_state_broadcaster.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index 8527008551..2e159e9a4f 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -364,15 +364,8 @@ controller_interface::return_type BatteryStateBroadcaster::update( } if (interfaces.battery_present) { - auto opt = state_interfaces_[interface_cnt].get_optional(); - if (opt.has_value()) - { - raw_battery_states_msg.battery_states[i].present = opt->value(); - } - else - { - raw_battery_states_msg.battery_states[i].present = false; - } + raw_battery_states_msg.battery_states[i].present = + state_interfaces_[interface_cnt].get_optional().value_or(false); interface_cnt++; } else From f90eca9b88c0131459697561cdf458e05e20e0ea Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 3 Oct 2025 00:18:18 +0300 Subject: [PATCH 12/19] add compatibility for ipa320 broadcaster --- .../src/battery_state_broadcaster.cpp | 58 +++++++++++++++---- .../src/battery_state_broadcaster.yaml | 24 +++++++- 2 files changed, 70 insertions(+), 12 deletions(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index 2e159e9a4f..d7cf5a86d2 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -50,8 +50,27 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - state_joints_ = params_.state_joints; - battery_presence_.resize(state_joints_.size(), false); + if (!params_.sensor_name.empty()) + { + if (params_.state_joints.size() > 0) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "You cannot use both 'sensor_name' and 'state_joints' parameters. Please use only " + "'state_joints' going forward."); + return controller_interface::CallbackReturn::ERROR; + } + RCLCPP_WARN( + get_node()->get_logger(), + "The 'sensor_name' parameter is deprecated and will be removed in future releases. Please " + "use 'state_joints' parameter instead."); + state_joints_ = {params_.sensor_name}; + } + else + { + state_joints_ = params_.state_joints; + } + battery_presence_.resize(params_.state_joints.size(), false); try { @@ -86,8 +105,8 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( raw_battery_states_realtime_publisher_->lock(); auto & msg = raw_battery_states_realtime_publisher_->msg_; - msg.battery_states.reserve(state_joints_.size()); - for (size_t i = 0; i < state_joints_.size(); ++i) + msg.battery_states.reserve(params_.state_joints.size()); + for (size_t i = 0; i < params_.state_joints.size(); ++i) { sensor_msgs::msg::BatteryState battery; battery.location.reserve(MAX_LENGTH); @@ -97,7 +116,7 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( raw_battery_states_realtime_publisher_->unlock(); // Get count of enabled joints for each interface - for (size_t i = 0; i < state_joints_.size(); ++i) + for (size_t i = 0; i < params_.state_joints.size(); ++i) { const auto & interfaces = params_.interfaces.state_joints_map.at(params_.state_joints.at(i)); const auto & battery_properties = params_.state_joints_map.at(params_.state_joints.at(i)); @@ -152,8 +171,15 @@ BatteryStateBroadcaster::state_interface_configuration() const state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_joints_.size() * 8); - for (const auto & joint : state_joints_) + if (!params_.sensor_name.empty()) + { + state_interfaces_config.names.reserve(1); + state_interfaces_config.names.push_back(params_.sensor_name + "/voltage"); + return state_interfaces_config; + } + + state_interfaces_config.names.reserve(params_.state_joints.size() * 8); + for (const auto & joint : params_.state_joints) { const auto & interfaces = params_.interfaces.state_joints_map.at(joint); state_interfaces_config.names.push_back(joint + "/battery_voltage"); @@ -210,12 +236,12 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( // handle individual battery states initializations auto & raw_battery_states_msg = raw_battery_states_realtime_publisher_->msg_; - for (size_t i = 0; i < state_joints_.size(); ++i) + for (size_t i = 0; i < params_.state_joints.size(); ++i) { auto & battery_state = raw_battery_states_msg.battery_states[i]; const auto & battery_properties = params_.state_joints_map.at(params_.state_joints.at(i)); - battery_state.header.frame_id = state_joints_[i]; + battery_state.header.frame_id = params_.state_joints[i]; battery_state.voltage = kUninitializedValue; battery_state.temperature = kUninitializedValue; battery_state.current = kUninitializedValue; @@ -242,6 +268,12 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( combined_serial_number += battery_state.serial_number + ", "; } + if (!params_.sensor_name.empty()) + { + sums_.design_capacity_sum = static_cast(params_.design_capacity); + combined_power_supply_technology = static_cast(params_.power_supply_technology); + } + // handle aggregate battery state initialization auto & battery_state_msg = battery_state_realtime_publisher_->msg_; @@ -285,7 +317,7 @@ controller_interface::return_type BatteryStateBroadcaster::update( if (raw_battery_states_realtime_publisher_ && raw_battery_states_realtime_publisher_->trylock()) { auto & raw_battery_states_msg = raw_battery_states_realtime_publisher_->msg_; - for (size_t i = 0; i < state_joints_.size(); ++i) + for (size_t i = 0; i < params_.state_joints.size(); ++i) { const auto & interfaces = params_.interfaces.state_joints_map.at(params_.state_joints.at(i)); @@ -385,6 +417,12 @@ controller_interface::return_type BatteryStateBroadcaster::update( raw_battery_states_realtime_publisher_->unlockAndPublish(); } + if (!params_.sensor_name.empty()) + { + sums_.voltage_sum = + static_cast(state_interfaces_[0].get_optional().value_or(kUninitializedValue)); + } + if (battery_state_realtime_publisher_ && battery_state_realtime_publisher_->trylock()) { auto & battery_state_msg = battery_state_realtime_publisher_->msg_; diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.yaml b/battery_state_broadcaster/src/battery_state_broadcaster.yaml index bc4708deec..d25d0ab94b 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.yaml +++ b/battery_state_broadcaster/src/battery_state_broadcaster.yaml @@ -18,8 +18,7 @@ battery_state_broadcaster: description: "List of joints from which battery state interfaces will be read.", read_only: true, validation: { - unique<>: null, - size_gt<>: [0] + unique<>: null } } interfaces: @@ -105,3 +104,24 @@ battery_state_broadcaster: description: "Serial number of the battery.", read_only: true, } + sensor_name: { + type: string, + default_value: "", + description: "Sensor name of the battery. If provided, the 'voltage' state interface of this sensor will be used to populate the voltage field in the BatteryState message. If this parameter is used, the state joints and interfaces parameters are ignored.", + read_only: true, + } + design_capacity: { + type: double, + default_value: .nan, + description: "Design capacity of the battery [Ah] for the sensor_name mode (If unmeasured NaN).", + read_only: true, + } + power_supply_technology: { + type: int, + default_value: 0, + description: "Battery chemistry type as an enum for the sensor_name mode. If unmeasured, the technology is set to unknown.", + read_only: true, + validation: { + bounds<>: [0, 8] + } + } From 38891c80345736454e91f125052f3a87ab07c178 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 3 Oct 2025 00:48:06 +0300 Subject: [PATCH 13/19] fix compatibility errors --- .../src/battery_state_broadcaster.cpp | 23 +++++++++++-------- .../src/battery_state_broadcaster.yaml | 1 + 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index d7cf5a86d2..52b86c1d01 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -225,12 +225,21 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( return controller_interface::CallbackReturn::FAILURE; } - param_listener_->refresh_dynamic_parameters(); - // get parameters from the listener in case they were updated + param_listener_->refresh_dynamic_parameters(); params_ = param_listener_->get_params(); - auto combined_power_supply_technology = static_cast( - params_.state_joints_map.at(params_.state_joints.at(0)).power_supply_technology); + + uint8_t combined_power_supply_technology; + if (!params_.sensor_name.empty()) + { + sums_.design_capacity_sum = static_cast(params_.design_capacity); + combined_power_supply_technology = static_cast(params_.power_supply_technology); + } + else + { + combined_power_supply_technology = static_cast( + params_.state_joints_map.at(params_.state_joints.at(0)).power_supply_technology); + } std::string combined_location = ""; std::string combined_serial_number = ""; @@ -268,12 +277,6 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_activate( combined_serial_number += battery_state.serial_number + ", "; } - if (!params_.sensor_name.empty()) - { - sums_.design_capacity_sum = static_cast(params_.design_capacity); - combined_power_supply_technology = static_cast(params_.power_supply_technology); - } - // handle aggregate battery state initialization auto & battery_state_msg = battery_state_realtime_publisher_->msg_; diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.yaml b/battery_state_broadcaster/src/battery_state_broadcaster.yaml index d25d0ab94b..505aba8141 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.yaml +++ b/battery_state_broadcaster/src/battery_state_broadcaster.yaml @@ -17,6 +17,7 @@ battery_state_broadcaster: type: string_array, description: "List of joints from which battery state interfaces will be read.", read_only: true, + default_value: [], validation: { unique<>: null } From 60303f5bfade4d8d483c067aff70ba651f20139e Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 3 Oct 2025 01:17:16 +0300 Subject: [PATCH 14/19] add migration guide for ip320 users --- battery_state_broadcaster/doc/userdoc.rst | 37 +++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst index 8ddf46810e..0a69ba9f5a 100644 --- a/battery_state_broadcaster/doc/userdoc.rst +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -86,3 +86,40 @@ An example parameter file for this controller is available in the `test director .. 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. From ba45d443253f0860cef455ac404fd80f5cf56fda Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Tue, 7 Oct 2025 16:53:32 +0300 Subject: [PATCH 15/19] update doxygen broadcaster header --- .../battery_state_broadcaster.hpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index 372a45d6f3..e7793d40cd 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -38,14 +38,15 @@ namespace battery_state_broadcaster * \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: - * /voltage - * /current - * /charge - * /percentage - * /power_supply_status - * /power_supply_health - * /present + * The following state interfaces can be published: + * /battery_voltage (Mandatory) + * /battery_temperature + * /battery_current + * /battery_charge + * /battery_percentage + * /battery_power_supply_status + * /battery_power_supply_health + * /battery_present * * \param state_joints of the batteries to publish. * \param capacity of the batteries to publish. From 1393e76c5413270ac7979b449ba7cd4e07a7ae17 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 24 Oct 2025 12:32:35 +0300 Subject: [PATCH 16/19] remove license from yaml --- .../src/battery_state_broadcaster.yaml | 14 -------------- .../test/battery_state_broadcaster_params.yaml | 14 -------------- 2 files changed, 28 deletions(-) diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.yaml b/battery_state_broadcaster/src/battery_state_broadcaster.yaml index 505aba8141..1012a2e567 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.yaml +++ b/battery_state_broadcaster/src/battery_state_broadcaster.yaml @@ -1,17 +1,3 @@ -# 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. - battery_state_broadcaster: state_joints: { type: string_array, diff --git a/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml b/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml index 2efa5fc8f2..9e2837aedf 100644 --- a/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml +++ b/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml @@ -1,17 +1,3 @@ -# 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. - test_battery_state_broadcaster: ros__parameters: state_joints: From 1cdb4e492b80833096558b2965a1fd90f3b291df Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 24 Oct 2025 12:43:51 +0300 Subject: [PATCH 17/19] update build files with best practices --- battery_state_broadcaster/CMakeLists.txt | 22 ++++++++++++++-------- battery_state_broadcaster/package.xml | 11 +++++++---- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt index 86b5c263ff..0bacd8da37 100644 --- a/battery_state_broadcaster/CMakeLists.txt +++ b/battery_state_broadcaster/CMakeLists.txt @@ -1,33 +1,38 @@ 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() +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces - control_msgs controller_interface + hardware_interface generate_parameter_library pluginlib + rclcpp rclcpp_lifecycle realtime_tools sensor_msgs - control_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 +add_library( + battery_state_broadcaster + SHARED src/battery_state_broadcaster.cpp ) @@ -40,16 +45,17 @@ target_include_directories(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} - ${control_msgs_TARGETS} ${builtin_interfaces_TARGETS}) -pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml) +pluginlib_export_plugin_description_file( + controller_interface battery_state_broadcaster.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml index aef917c42a..634913b66c 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -17,16 +17,19 @@ https://github.com/ros-controls/ros2_controllers/ Yara Shahin - + ament_cmake + generate_parameter_library ros2_control_cmake - rosidl_default_generators - rclcpp + backward_ros controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle sensor_msgs - control_msgs rosidl_default_runtime From df64e08fb241928092cc3f3c810d1e41716c6016 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Fri, 24 Oct 2025 14:10:41 +0300 Subject: [PATCH 18/19] change message type to sensor messages --- battery_state_broadcaster/battery_state_broadcaster.xml | 2 +- battery_state_broadcaster/doc/userdoc.rst | 2 +- .../battery_state_broadcaster.hpp | 7 +++---- .../src/battery_state_broadcaster.cpp | 4 ++-- .../test/test_battery_state_broadcaster.hpp | 8 ++++---- 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/battery_state_broadcaster/battery_state_broadcaster.xml b/battery_state_broadcaster/battery_state_broadcaster.xml index 11bad6658b..14751c4da7 100644 --- a/battery_state_broadcaster/battery_state_broadcaster.xml +++ b/battery_state_broadcaster/battery_state_broadcaster.xml @@ -20,7 +20,7 @@ limitations under the License. type="battery_state_broadcaster::BatteryStateBroadcaster" base_class_type="controller_interface::ControllerInterface"> 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. + It also publishes the aggregated battery state of all joints as a single sensor_msgs/BatteryStates message. diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst index 0a69ba9f5a..36551bfa5c 100644 --- a/battery_state_broadcaster/doc/userdoc.rst +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -27,7 +27,7 @@ Published Topics The broadcaster publishes two topics: -- ``~/raw_battery_states`` (``control_msgs/msg/BatteryStates``) +- ``~/raw_battery_states`` (``sensor_msgs/msg/BatteryStates``) Publishes **per-joint battery state messages**, containing the raw values for each configured joint. - ``~/battery_state`` (``sensor_msgs/msg/BatteryState``) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index e7793d40cd..b0a4f13948 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -29,8 +29,8 @@ #include "realtime_tools/realtime_publisher.hpp" #include -#include "control_msgs/msg/battery_states.hpp" #include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/battery_states.hpp" namespace battery_state_broadcaster { @@ -93,7 +93,7 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> battery_state_realtime_publisher_; - std::shared_ptr> + std::shared_ptr> raw_battery_states_realtime_publisher_; struct BatteryInterfaceSums { @@ -119,8 +119,7 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface private: std::shared_ptr param_listener_; std::shared_ptr> battery_state_publisher_; - std::shared_ptr> - raw_battery_states_publisher_; + std::shared_ptr> raw_battery_states_publisher_; std::vector battery_presence_; }; diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index 52b86c1d01..f25813a3cf 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -81,11 +81,11 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( std::make_shared>( battery_state_publisher_); - raw_battery_states_publisher_ = get_node()->create_publisher( + raw_battery_states_publisher_ = get_node()->create_publisher( "~/raw_battery_states", rclcpp::SystemDefaultsQoS()); raw_battery_states_realtime_publisher_ = - std::make_shared>( + std::make_shared>( raw_battery_states_publisher_); } catch (const std::exception & e) diff --git a/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp index a3fd271935..c12ce9d9bd 100644 --- a/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp @@ -34,11 +34,11 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "control_msgs/msg/battery_states.hpp" #include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/battery_states.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; -using RawBatteryStatesMsg = control_msgs::msg::BatteryStates; +using RawBatteryStatesMsg = sensor_msgs::msg::BatteryStates; using sensor_msgs::msg::BatteryState; using testing::IsEmpty; using testing::SizeIs; @@ -108,7 +108,7 @@ class BatteryStateBroadcasterTest : public ::testing::Test protected: // Controller-related parameters std::vector state_joint_names_ = {"left_wheel", "right_wheel"}; - std::array itfs_values_ = { + std::array itfs_values_ = {{ 5.0, // 0 left_voltage 60.0, // 1 left_temperature 6000.0, // 2 left_charge @@ -121,7 +121,7 @@ class BatteryStateBroadcasterTest : public ::testing::Test 66.0, // 9 right_percentage 2.0, // 10 right_status 4.0 // 11 right_health - }; + }}; // std::array itfs_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; hardware_interface::StateInterface left_voltage_itf_{ From 15cebde2ac303b51fbf824064eb5afad64e8f4f6 Mon Sep 17 00:00:00 2001 From: Yara Shahin Date: Wed, 12 Nov 2025 18:11:30 +0200 Subject: [PATCH 19/19] rename msg to BatteryStateArray --- .../battery_state_broadcaster.xml | 2 +- battery_state_broadcaster/doc/userdoc.rst | 2 +- .../battery_state_broadcaster.hpp | 11 ++++++----- .../src/battery_state_broadcaster.cpp | 7 ++++--- .../test/test_battery_state_broadcaster.hpp | 4 ++-- 5 files changed, 14 insertions(+), 12 deletions(-) diff --git a/battery_state_broadcaster/battery_state_broadcaster.xml b/battery_state_broadcaster/battery_state_broadcaster.xml index 14751c4da7..38c1236119 100644 --- a/battery_state_broadcaster/battery_state_broadcaster.xml +++ b/battery_state_broadcaster/battery_state_broadcaster.xml @@ -20,7 +20,7 @@ limitations under the License. type="battery_state_broadcaster::BatteryStateBroadcaster" base_class_type="controller_interface::ControllerInterface"> 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/BatteryStates message. + It also publishes the aggregated battery state of all joints as a single sensor_msgs/BatteryStateArray message. diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst index 36551bfa5c..721afa6c05 100644 --- a/battery_state_broadcaster/doc/userdoc.rst +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -27,7 +27,7 @@ Published Topics The broadcaster publishes two topics: -- ``~/raw_battery_states`` (``sensor_msgs/msg/BatteryStates``) +- ``~/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``) diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp index b0a4f13948..a2f19dd7b7 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -30,7 +30,7 @@ #include #include "sensor_msgs/msg/battery_state.hpp" -#include "sensor_msgs/msg/battery_states.hpp" +#include "sensor_msgs/msg/battery_state_array.hpp" namespace battery_state_broadcaster { @@ -59,8 +59,8 @@ namespace battery_state_broadcaster * * - \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. + * - \b raw_battery_states (battery_state_broadcaster::msg::BatteryStateArray): battery states of + * the individual battery joints. * */ class BatteryStateBroadcaster : public controller_interface::ControllerInterface @@ -93,7 +93,7 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> battery_state_realtime_publisher_; - std::shared_ptr> + std::shared_ptr> raw_battery_states_realtime_publisher_; struct BatteryInterfaceSums { @@ -119,7 +119,8 @@ class BatteryStateBroadcaster : public controller_interface::ControllerInterface private: std::shared_ptr param_listener_; std::shared_ptr> battery_state_publisher_; - std::shared_ptr> raw_battery_states_publisher_; + std::shared_ptr> + raw_battery_states_publisher_; std::vector battery_presence_; }; diff --git a/battery_state_broadcaster/src/battery_state_broadcaster.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp index f25813a3cf..759d0ff3c5 100644 --- a/battery_state_broadcaster/src/battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -81,11 +81,12 @@ controller_interface::CallbackReturn BatteryStateBroadcaster::on_configure( std::make_shared>( battery_state_publisher_); - raw_battery_states_publisher_ = get_node()->create_publisher( - "~/raw_battery_states", rclcpp::SystemDefaultsQoS()); + raw_battery_states_publisher_ = + get_node()->create_publisher( + "~/raw_battery_states", rclcpp::SystemDefaultsQoS()); raw_battery_states_realtime_publisher_ = - std::make_shared>( + std::make_shared>( raw_battery_states_publisher_); } catch (const std::exception & e) diff --git a/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp index c12ce9d9bd..816537a67e 100644 --- a/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp @@ -35,10 +35,10 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "sensor_msgs/msg/battery_states.hpp" +#include "sensor_msgs/msg/battery_state_array.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; -using RawBatteryStatesMsg = sensor_msgs::msg::BatteryStates; +using RawBatteryStatesMsg = sensor_msgs::msg::BatteryStateArray; using sensor_msgs::msg::BatteryState; using testing::IsEmpty; using testing::SizeIs;