diff --git a/battery_state_broadcaster/CHANGELOG.rst b/battery_state_broadcaster/CHANGELOG.rst index ffc7e8f7bf..bd5a156727 100644 --- a/battery_state_broadcaster/CHANGELOG.rst +++ b/battery_state_broadcaster/CHANGELOG.rst @@ -2,15 +2,19 @@ Changelog for package battery_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The entries below refer to the standalone `ipa320/ros_battery_monitoring `_ package, +from which this broadcaster originates. + 1.1.0 (2025-09-26) ------------------ * address deprecations in ros2_control for kilted -* Don't make a temporary copy of semantic component (https://github.com/ipa320/ros_battery_monitoring/pull/9) +* Don't make a temporary copy of semantic component (`#9 `_) * Contributors: Christoph Froehlich, Jonas Otto 1.0.2 (2025-06-01) ------------------ -* Replace ament_target_dependencies with target_link_libraries ([#6](https://github.com/ipa320/ros_battery_monitoring/issues/6)) +* Replace ament_target_dependencies with target_link_libraries (`#6 `_) * Contributors: Alejandro Hernandez Cordero, Jonas Otto 1.0.1 (2025-02-06) diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt index bfc1e53a8c..da65d4d559 100644 --- a/battery_state_broadcaster/CMakeLists.txt +++ b/battery_state_broadcaster/CMakeLists.txt @@ -1,54 +1,86 @@ cmake_minimum_required(VERSION 3.10) project(battery_state_broadcaster) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS + builtin_interfaces controller_interface + hardware_interface + generate_parameter_library pluginlib + rclcpp + rclcpp_lifecycle realtime_tools sensor_msgs + urdf ) -# find dependencies find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(sensor_msgs REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) -add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp) -target_include_directories(battery_state_broadcaster PUBLIC - $ - $ +generate_parameter_library(battery_state_broadcaster_parameters + src/battery_state_broadcaster_parameters.yaml ) -target_link_libraries(battery_state_broadcaster + +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 - controller_interface::controller_interface - pluginlib::pluginlib - realtime_tools::realtime_tools - ${sensor_msgs_TARGETS} + $ + $ ) +target_link_libraries(battery_state_broadcaster PUBLIC + battery_state_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS} + ${builtin_interfaces_TARGETS}) -pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml) + +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(rclcpp 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 hardware_interface::hardware_interface rclcpp::rclcpp 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( @@ -57,15 +89,14 @@ install( ) install( TARGETS - battery_state_broadcaster + battery_state_broadcaster + battery_state_broadcaster_parameters EXPORT export_battery_state_broadcaster RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - INCLUDES DESTINATION include ) ament_export_targets(export_battery_state_broadcaster HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - ament_package() diff --git a/battery_state_broadcaster/battery_state_broadcaster.xml b/battery_state_broadcaster/battery_state_broadcaster.xml index e4d371b578..f4ac2ce3d5 100644 --- a/battery_state_broadcaster/battery_state_broadcaster.xml +++ b/battery_state_broadcaster/battery_state_broadcaster.xml @@ -1,8 +1,9 @@ - - - This controller publishes the readings of a battery sensor as sensor_msgs/BatteryState message. - - - \ No newline at end of file + + + This controller publishes the individual battery state of each battery as sensor_msgs/BatteryState messages. + It also publishes the aggregated battery state of all batteries as a single control_msgs/BatteryStateArray message. + + + diff --git a/battery_state_broadcaster/doc/userdoc.rst b/battery_state_broadcaster/doc/userdoc.rst index 85c091c5d7..f54eb459c1 100644 --- a/battery_state_broadcaster/doc/userdoc.rst +++ b/battery_state_broadcaster/doc/userdoc.rst @@ -3,53 +3,123 @@ .. _battery_state_broadcaster_userdoc: Battery State Broadcaster +-------------------------------- +The *Battery State Broadcaster* publishes battery status information as ``sensor_msgs/msg/BatteryState`` messages. + +It reads battery-related state interfaces from one or more batteries 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 battery: + +- ``battery_voltage`` *(mandatory)* (double) +- ``battery_temperature`` *(optional)* (double) +- ``battery_current`` *(optional)* (double) +- ``battery_charge`` *(optional)* (double) +- ``battery_percentage`` *(optional)* (double) +- ``battery_power_supply_status`` *(optional)* (double) +- ``battery_power_supply_health`` *(optional)* (double) +- ``battery_present`` *(optional)* (bool) + +Published Topics +^^^^^^^^^^^^^^^^^^ + +The broadcaster publishes two topics: + +- ``~/raw_battery_states`` (``control_msgs/msg/BatteryStateArray``) + Publishes **per-battery state messages**, containing the raw values for each configured battery. + +- ``~/battery_state`` (``sensor_msgs/msg/BatteryState``) + Publishes a **single aggregated battery message** representing the combined status across all batteries. + ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| Field | ``battery_state`` | ``raw_battery_states`` | ++=============================+=========================================================================+=================================================================================================================================================+ +| ``header.frame_id`` | Empty | Battery name | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``voltage`` | Mean across all batteries | From battery's ``battery_voltage`` interface if enabled, otherwise nan | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``temperature`` | Mean across batteries reporting temperature | From battery's ``battery_temperature`` interface if enabled, otherwise nan. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``current`` | Mean across batteries reporting current | From battery's ``battery_current`` interface if enabled, otherwise nan. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``charge`` | Sum across all batteries | From battery's ``battery_charge`` interface if enabled, otherwise nan. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``capacity`` | Sum across all batteries | From battery's ``capacity`` parameter if provided, otherwise nan. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``design_capacity`` | Sum across all batteries | From battery's ``design_capacity`` parameter if provided, otherwise nan. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``percentage`` | Mean across batteries reporting/calculating percentage | From battery's ``battery_percentage`` interface if enabled, otherwise calculated from battery's ``min_voltage`` and ``max_voltage`` parameters. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``power_supply_status`` | Highest reported enum value | From battery's ``battery_power_supply_status`` interface if enabled, otherwise 0 (unknown). | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``power_supply_health`` | Highest reported enum value | From battery's ``battery_power_supply_health`` interface if enabled, otherwise 0 (unknown). | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``power_supply_technology`` | Reported as-is if same across all batteries, otherwise set to *Unknown* | From battery's ``power_supply_technology`` parameter if provided, otherwise 0 (unknown). | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``present`` | True | From battery's ``battery_present`` interface if enabled, otherwise true if battery's voltage values is valid. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``cell_voltage`` | Empty | Empty | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``cell_temperature`` | Empty | Empty | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``location`` | All battery locations appended | From battery's ``location`` parameter if provided, otherwise empty. | ++-----------------------------+-------------------------------------------------------------------------+-------------------------------------------------------------------------------------------------------------------------------------------------+ +| ``serial_number`` | All battery serial numbers appended | From battery'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 ========================= -This broadcaster publishes `sensor_msgs/BatteryState `__ messages from appropriate state interfaces. +.. generate_parameter_library_details:: ../src/battery_state_broadcaster_parameters.yaml +Example Parameter File +========================= -Required State Interfaces ---------------------------------- -This broadcaster requires the robot to have a sensor component which contains the battery state interfaces: +An example parameter file for this controller is available in the `test directory `_: -.. code-block:: xml +.. 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: -Parameters ---------------------------------- -To use this broadcaster, declare it in the controller manager and set its parameters: +1. Update your hardware interface name from ``voltage`` → ``battery_voltage``. + +2. Convert your controller parameters from -.. code-block:: yaml + .. code-block:: yaml - controller_manager: - ros__parameters: - battery_state_broadcaster: - type: battery_state_broadcaster/BatteryStateBroadcaster + battery_state_broadcaster: + ros__parameters: + sensor_name: "battery_state" + design_capacity: 100.0 + # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg + power_supply_technology: 2 - battery_state_broadcaster: - ros__parameters: - sensor_name: "battery_state" - design_capacity: 100.0 - # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg - power_supply_technology: 2 + to: -And spawn it in the launch file: + .. code-block:: yaml -.. code-block:: python + battery_state_broadcaster: + ros__parameters: + batteries: ["battery_state"] + battery_state: + design_capacity: 100.0 + power_supply_technology: 2 - battery_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["battery_state_broadcaster"] +**Notes**: -Topics ---------------------------------- -The battery state is published on ``~/battery_state``. -Since it's a plugin within the controller manager, add a remapping of the form ``("~/battery_state", "/my_battery_state")`` to the *controller manager*, not the spawner, to change the topic name. +- Parameters must provide **either** sensor_name **or** batteries. +- If both are empty → the broadcaster will fail to configure. +- If both are set → the broadcaster will throw an error. diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp deleted file mode 100644 index eb3330c172..0000000000 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp +++ /dev/null @@ -1,36 +0,0 @@ - -#include -#include -#include -#include - -namespace battery_state_broadcaster -{ -class BatterySensor : public semantic_components::SemanticComponentInterface -{ -public: - explicit BatterySensor(const std::string& name) - : semantic_components::SemanticComponentInterface(name, 1) - { - interface_names_.emplace_back(name_ + "/" + "voltage"); - } - - virtual ~BatterySensor() = default; - - double get_voltage() - { - voltage_ = state_interfaces_[0].get().get_optional().value(); - return voltage_; - } - - bool get_values_as_message(sensor_msgs::msg::BatteryState& message) - { - get_voltage(); - message.voltage = static_cast(voltage_); - return true; - } - -private: - double voltage_ = 0.0; -}; -} // namespace battery_state_broadcaster diff --git a/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp index 16fb6a8022..c67258344d 100644 --- a/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp +++ b/battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp @@ -1,35 +1,23 @@ -#pragma once - -#include -#include -#include -#include - -#include "BatterySensor.hpp" - -namespace battery_state_broadcaster -{ -class BatteryStateBroadcaster : public controller_interface::ControllerInterface -{ -public: - [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - [[nodiscard]] 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; - -private: - rclcpp::Publisher::SharedPtr battery_state_pub_; - std::unique_ptr battery_sensor_; - std::unique_ptr> realtime_publisher_; - sensor_msgs::msg::BatteryState msg_; -}; -} // namespace battery_state_broadcaster +// 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__BATTERYSTATEBROADCASTER_HPP_ +#define BATTERY_STATE_BROADCASTER__BATTERYSTATEBROADCASTER_HPP_ + +#pragma message( \ + "BatteryStateBroadcaster.hpp is deprecated, please use battery_state_broadcaster.hpp instead.") + +#include "battery_state_broadcaster/battery_state_broadcaster.hpp" + +#endif // BATTERY_STATE_BROADCASTER__BATTERYSTATEBROADCASTER_HPP_ 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..5cc6d2f1e9 --- /dev/null +++ b/battery_state_broadcaster/include/battery_state_broadcaster/battery_state_broadcaster.hpp @@ -0,0 +1,133 @@ +// 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 + +#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_state_array.hpp" +#include "sensor_msgs/msg/battery_state.hpp" + +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 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 batteries names 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): combined battery state across all + * configured batteries. + * - \b raw_battery_states (control_msgs::msg::BatteryStateArray): battery states of + * the individual batteries. + * + */ +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; + +protected: + 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 + { + int temperature_cnt = 0; + int current_cnt = 0; + int percentage_cnt = 0; + }; + + battery_state_broadcaster::Params params_; + + std::vector batteries_; + + BatteryInterfaceSums sums_; + BatteryInterfaceCounts counts_; + +private: + std::shared_ptr> + battery_state_realtime_publisher_; + std::shared_ptr> + raw_battery_states_realtime_publisher_; + + std::shared_ptr param_listener_; + std::shared_ptr> battery_state_publisher_; + std::shared_ptr> + raw_battery_states_publisher_; + sensor_msgs::msg::BatteryState battery_state_msg; + control_msgs::msg::BatteryStateArray raw_battery_states_msg; + + std::vector battery_presence_; +}; + +} // 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 index 7d272eb508..6b7979c531 100644 --- a/battery_state_broadcaster/package.xml +++ b/battery_state_broadcaster/package.xml @@ -2,16 +2,34 @@ battery_state_broadcaster - 1.1.0 - ROS2 Control boradcaster for battery state sensors. - Jonas Otto - MIT + 0.1.0 + ros2_control battery state broadcaster controller + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Yara Shahin + Jonas Otto ament_cmake + generate_parameter_library + ros2_control_cmake + + backward_ros controller_interface + hardware_interface pluginlib - realtime_tools + rclcpp + rclcpp_lifecycle sensor_msgs ament_cmake_gmock diff --git a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp b/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp deleted file mode 100644 index f6432b3960..0000000000 --- a/battery_state_broadcaster/src/BatteryStateBroadcaster.cpp +++ /dev/null @@ -1,101 +0,0 @@ -#include "battery_state_broadcaster/BatteryStateBroadcaster.hpp" -#include -#include -#include - -namespace battery_state_broadcaster -{ -controller_interface::CallbackReturn BatteryStateBroadcaster::on_init() -{ - get_node()->declare_parameter("sensor_name", "battery_state"); - get_node()->declare_parameter("power_supply_technology", -1); - get_node()->declare_parameter("design_capacity", 0.0); - return CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -BatteryStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) -{ - std::string sensor_name = get_node()->get_parameter("sensor_name").as_string(); - - battery_sensor_ = std::make_unique(sensor_name); - - battery_state_pub_ = - get_node()->create_publisher("~/battery_state", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = - std::make_unique>(battery_state_pub_); - - msg_.temperature = std::numeric_limits::quiet_NaN(); - msg_.current = std::numeric_limits::quiet_NaN(); - msg_.charge = std::numeric_limits::quiet_NaN(); - msg_.capacity = std::numeric_limits::quiet_NaN(); - msg_.design_capacity = std::numeric_limits::quiet_NaN(); - msg_.percentage = std::numeric_limits::quiet_NaN(); - msg_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; - msg_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; - msg_.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; - msg_.present = true; - - int64_t psu_tech = get_node()->get_parameter("power_supply_technology").as_int(); - if (psu_tech != -1) - { - msg_.power_supply_technology = psu_tech; - } - - double design_capacity = get_node()->get_parameter("design_capacity").as_double(); - if (design_capacity != 0.0) - { - msg_.design_capacity = static_cast(design_capacity); - } - - return CallbackReturn::SUCCESS; -} - -[[nodiscard]] controller_interface::InterfaceConfiguration -BatteryStateBroadcaster::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; - return command_interfaces_config; -} - -[[nodiscard]] 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 = battery_sensor_->get_state_interface_names(); - return state_interfaces_config; -} - -controller_interface::CallbackReturn -BatteryStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - battery_sensor_->assign_loaned_state_interfaces(state_interfaces_); - return CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -BatteryStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - battery_sensor_->release_interfaces(); - return CallbackReturn::SUCCESS; -} - -controller_interface::return_type BatteryStateBroadcaster::update(const rclcpp::Time& time, - const rclcpp::Duration& /*period*/) -{ - if (realtime_publisher_) - { - msg_.header.stamp = time; - battery_sensor_->get_values_as_message(msg_); - realtime_publisher_->try_publish(msg_); - } - - return controller_interface::return_type::OK; -} - -} // 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.cpp b/battery_state_broadcaster/src/battery_state_broadcaster.cpp new file mode 100644 index 0000000000..496cf7aeb9 --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster.cpp @@ -0,0 +1,458 @@ +// 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; // maximum length of strings to reserve + +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(); + if (!params_.sensor_name.empty()) + { + if (params_.batteries.size() > 0) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "You cannot use both 'sensor_name' and 'batteries' parameters. Please use only " + "'batteries' 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 'batteries' parameter instead."); + batteries_ = {params_.sensor_name}; + } + else + { + batteries_ = params_.batteries; + } + battery_presence_.resize(params_.batteries.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_msg.location.reserve(MAX_LENGTH); + battery_state_msg.serial_number.reserve(MAX_LENGTH); + + raw_battery_states_msg.battery_states.reserve(params_.batteries.size()); + for (size_t i = 0; i < params_.batteries.size(); ++i) + { + sensor_msgs::msg::BatteryState battery; + battery.location.reserve(MAX_LENGTH); + battery.serial_number.reserve(MAX_LENGTH); + raw_battery_states_msg.battery_states.emplace_back(std::move(battery)); + } + + // Get count of enabled batteries for each interface + for (size_t i = 0; i < params_.batteries.size(); ++i) + { + const auto & interfaces = params_.interfaces.batteries_map.at(params_.batteries.at(i)); + const auto & battery_properties = params_.batteries_map.at(params_.batteries.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; + + 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_.batteries.size() * 8); + for (const auto & battery : params_.batteries) + { + const auto & interfaces = params_.interfaces.batteries_map.at(battery); + state_interfaces_config.names.push_back(battery + "/battery_voltage"); + if (interfaces.battery_temperature) + { + state_interfaces_config.names.push_back(battery + "/battery_temperature"); + } + if (interfaces.battery_current) + { + state_interfaces_config.names.push_back(battery + "/battery_current"); + } + if (interfaces.battery_charge) + { + state_interfaces_config.names.push_back(battery + "/battery_charge"); + } + if (interfaces.battery_percentage) + { + state_interfaces_config.names.push_back(battery + "/battery_percentage"); + } + if (interfaces.battery_power_supply_status) + { + state_interfaces_config.names.push_back(battery + "/battery_power_supply_status"); + } + if (interfaces.battery_power_supply_health) + { + state_interfaces_config.names.push_back(battery + "/battery_power_supply_health"); + } + if (interfaces.battery_present) + { + state_interfaces_config.names.push_back(battery + "/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; + } + + // get parameters from the listener in case they were updated + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); + + 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_.batteries_map.at(params_.batteries.at(0)).power_supply_technology); + } + std::string combined_location = ""; + std::string combined_serial_number = ""; + + // handle individual battery states initializations + for (size_t i = 0; i < params_.batteries.size(); ++i) + { + auto & battery_state = raw_battery_states_msg.battery_states[i]; + const auto & battery_properties = params_.batteries_map.at(params_.batteries.at(i)); + + battery_state.header.frame_id = params_.batteries[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 + ", "; + } + raw_battery_states_realtime_publisher_->try_publish(raw_battery_states_msg); + + // handle aggregate battery state initialization + 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; + + battery_state_realtime_publisher_->try_publish(battery_state_msg); + + 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_ = {}; + std::size_t 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_) + { + for (size_t i = 0; i < params_.batteries.size(); ++i) + { + const auto & interfaces = params_.interfaces.batteries_map.at(params_.batteries.at(i)); + + raw_battery_states_msg.battery_states[i].header.stamp = time; + + 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 = 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 = 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 = 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 = static_cast( + state_interfaces_[interface_cnt].get_optional().value_or(kUninitializedValue)); + sums_.percentage_sum += raw_battery_states_msg.battery_states[i].percentage; + interface_cnt++; + } + else + { + auto min_volt = params_.batteries_map.at(params_.batteries.at(i)).minimum_voltage; + auto max_volt = params_.batteries_map.at(params_.batteries.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 = + 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) + { + 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 = + 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) + { + combined_power_supply_health = + raw_battery_states_msg.battery_states[i].power_supply_health; + } + interface_cnt++; + } + if (interfaces.battery_present) + { + raw_battery_states_msg.battery_states[i].present = + state_interfaces_[interface_cnt].get_optional().value_or(false); + interface_cnt++; + } + else + { + if ( + (!std::isnan(raw_battery_states_msg.battery_states[i].voltage)) && + (raw_battery_states_msg.battery_states[i].voltage != 0.0f)) + { + raw_battery_states_msg.battery_states[i].present = true; + } + else + { + raw_battery_states_msg.battery_states[i].present = false; + } + } + } + raw_battery_states_realtime_publisher_->try_publish(raw_battery_states_msg); + } + + 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_msg.header.stamp = time; + battery_state_msg.voltage = sums_.voltage_sum / static_cast(batteries_.size()); + + if (counts_.temperature_cnt) + { + battery_state_msg.temperature = + sums_.temperature_sum / static_cast(counts_.temperature_cnt); + } + if (counts_.current_cnt) + { + battery_state_msg.current = sums_.current_sum / static_cast(counts_.current_cnt); + } + battery_state_msg.charge = sums_.charge_sum; + if (counts_.percentage_cnt) + { + battery_state_msg.percentage = + sums_.percentage_sum / static_cast(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_->try_publish(battery_state_msg); + } + + return controller_interface::return_type::OK; +} + +} // 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_parameters.yaml b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml new file mode 100644 index 0000000000..2ea60e7e4d --- /dev/null +++ b/battery_state_broadcaster/src/battery_state_broadcaster_parameters.yaml @@ -0,0 +1,114 @@ +battery_state_broadcaster: + batteries: { + type: string_array, + description: "List of batteries from which battery state interfaces will be read.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + interfaces: + __map_batteries: + battery_temperature: { + type: bool, + default_value: false, + description: "Whether to read battery temperature [°C] from this battery's state interface (If unmeasured NaN)." + } + battery_current: { + type: bool, + default_value: false, + description: "Whether to read battery current [A] from this battery's state interface (If unmeasured NaN)." + } + battery_charge: { + type: bool, + default_value: false, + description: "Whether to read battery charge [Ah] from this battery'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 battery'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, see https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg) from this battery'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, see https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg) from this battery'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 battery's state interface. If unmeasured, the presence will be set to true if any other state interfaces from this battery are available." + } + __map_batteries: + 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 (see https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg). 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, + } + sensor_name: { + type: string, + default_value: "", + description: "[DEPRECATED] 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 batteries and interfaces parameters are ignored.", + read_only: true, + } + design_capacity: { + type: double, + default_value: .nan, + description: "[DEPRECATED] 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: "[DEPRECATED] Battery chemistry type as an enum for the sensor_name mode (see https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg). If unmeasured, the technology is set to unknown.", + read_only: true, + validation: { + bounds<>: [0, 8] + } + } 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..1787bb5a19 --- /dev/null +++ b/battery_state_broadcaster/test/battery_state_broadcaster_params.yaml @@ -0,0 +1,38 @@ +test_battery_state_broadcaster: + ros__parameters: + batteries: + - "battery0" + - "battery1" + interfaces: + battery0: + 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 + battery1: + 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 + battery0: + minimum_voltage: 0.0 + maximum_voltage: 10.0 + capacity: 12000.0 + design_capacity: 13000.0 + power_supply_technology: 3 + location: "slot0" + serial_number: "serial_device_0" + battery1: + minimum_voltage: 0.0 + maximum_voltage: 15.0 + capacity: 17000.0 + design_capacity: 18000.0 + power_supply_technology: 3 + location: "slot1" + serial_number: "serial_device_1" 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..8539e7e6b1 --- /dev/null +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp @@ -0,0 +1,306 @@ +// 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_.batteries.empty()); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + + ASSERT_THAT(battery_state_broadcaster_->batteries_, testing::ElementsAreArray(battery_names_)); + + auto interface_params = battery_state_broadcaster_->params_.interfaces.batteries_map; + auto properties = battery_state_broadcaster_->params_.batteries_map; + EXPECT_EQ(interface_params.at("battery0").battery_temperature, true); + EXPECT_EQ(interface_params.at("battery0").battery_current, false); + EXPECT_EQ(interface_params.at("battery0").battery_charge, true); + EXPECT_EQ(interface_params.at("battery0").battery_percentage, false); + EXPECT_EQ(interface_params.at("battery0").battery_power_supply_status, true); + EXPECT_EQ(interface_params.at("battery0").battery_power_supply_health, true); + EXPECT_EQ(interface_params.at("battery0").battery_present, false); + + EXPECT_EQ(interface_params.at("battery1").battery_temperature, true); + EXPECT_EQ(interface_params.at("battery1").battery_current, true); + EXPECT_EQ(interface_params.at("battery1").battery_charge, true); + EXPECT_EQ(interface_params.at("battery1").battery_percentage, true); + EXPECT_EQ(interface_params.at("battery1").battery_power_supply_status, true); + EXPECT_EQ(interface_params.at("battery1").battery_power_supply_health, true); + EXPECT_EQ(interface_params.at("battery1").battery_present, false); + + EXPECT_EQ(properties.at("battery0").minimum_voltage, 0.0); + EXPECT_EQ(properties.at("battery0").maximum_voltage, 10.0); + EXPECT_EQ(properties.at("battery0").capacity, 12000.0); + EXPECT_EQ(properties.at("battery0").design_capacity, 13000.0); + EXPECT_EQ(properties.at("battery0").power_supply_technology, 3); + EXPECT_EQ(properties.at("battery0").location, "slot0"); + EXPECT_EQ(properties.at("battery0").serial_number, "serial_device_0"); + + EXPECT_EQ(properties.at("battery1").minimum_voltage, 0.0); + EXPECT_EQ(properties.at("battery1").maximum_voltage, 15.0); + EXPECT_EQ(properties.at("battery1").capacity, 17000.0); + EXPECT_EQ(properties.at("battery1").design_capacity, 18000.0); + EXPECT_EQ(properties.at("battery1").power_supply_technology, 3); + EXPECT_EQ(properties.at("battery1").location, "slot1"); + EXPECT_EQ(properties.at("battery1").serial_number, "serial_device_1"); + + // check property aggregation + EXPECT_EQ(battery_state_broadcaster_->counts_.temperature_cnt, 2); + EXPECT_EQ(battery_state_broadcaster_->counts_.current_cnt, 1); + EXPECT_EQ( + battery_state_broadcaster_->counts_.percentage_cnt, + 2); // 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) +{ + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_battery_state_broadcaster"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = battery_state_broadcaster_->define_custom_node_options(); + ASSERT_EQ(battery_state_broadcaster_->init(params), controller_interface::return_type::OK); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_FALSE(activate_succeeds(battery_state_broadcaster_)); +} + +TEST_F(BatteryStateBroadcasterTest, activate_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); +} + +TEST_F(BatteryStateBroadcasterTest, deactivate_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(deactivate_succeeds(battery_state_broadcaster_)); +} + +TEST_F(BatteryStateBroadcasterTest, check_exported_intefaces) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + + 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], "battery0/battery_voltage"); + EXPECT_EQ(state_interfaces.names[1], "battery0/battery_temperature"); + EXPECT_EQ(state_interfaces.names[2], "battery0/battery_charge"); + EXPECT_EQ(state_interfaces.names[3], "battery0/battery_power_supply_status"); + EXPECT_EQ(state_interfaces.names[4], "battery0/battery_power_supply_health"); + EXPECT_EQ(state_interfaces.names[5], "battery1/battery_voltage"); + EXPECT_EQ(state_interfaces.names[6], "battery1/battery_temperature"); + EXPECT_EQ(state_interfaces.names[7], "battery1/battery_current"); + EXPECT_EQ(state_interfaces.names[8], "battery1/battery_charge"); + EXPECT_EQ(state_interfaces.names[9], "battery1/battery_percentage"); + EXPECT_EQ(state_interfaces.names[10], "battery1/battery_power_supply_status"); + EXPECT_EQ(state_interfaces.names[11], "battery1/battery_power_supply_health"); +} + +TEST_F(BatteryStateBroadcasterTest, update_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); + + 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_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); + + 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); + + // battery0 + const auto & battery0 = raw_battery_states_msg.battery_states[0]; + EXPECT_EQ(battery0.header.frame_id, "battery0"); + EXPECT_DOUBLE_EQ(battery0.voltage, 5.0); + EXPECT_DOUBLE_EQ(battery0.temperature, 60.0); + EXPECT_TRUE(std::isnan(battery0.current)); // disabled in params + EXPECT_DOUBLE_EQ(battery0.charge, 6000.0); + EXPECT_DOUBLE_EQ(battery0.capacity, 12000.0); + EXPECT_DOUBLE_EQ(battery0.design_capacity, 13000.0); + // percentage calculated (no interface) = (5.0 - 0.0) * 100 / (10.0 - 0.0) = 50 + EXPECT_DOUBLE_EQ(battery0.percentage, 50.0); + EXPECT_EQ(battery0.power_supply_status, 3); // from itfs_values_[3] + EXPECT_EQ(battery0.power_supply_health, 0); // from itfs_values_[4] + EXPECT_EQ(battery0.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_TRUE(battery0.present); // voltage > 0.0 + EXPECT_EQ(battery0.location, "slot0"); + EXPECT_EQ(battery0.serial_number, "serial_device_0"); + + // battery1 + const auto & battery1 = raw_battery_states_msg.battery_states[1]; + EXPECT_EQ(battery1.header.frame_id, "battery1"); + EXPECT_DOUBLE_EQ(battery1.voltage, 10.0); + EXPECT_DOUBLE_EQ(battery1.temperature, 80.0); + EXPECT_DOUBLE_EQ(battery1.current, 2000.0); + EXPECT_DOUBLE_EQ(battery1.charge, 5000.0); + EXPECT_DOUBLE_EQ(battery1.capacity, 17000.0); + EXPECT_DOUBLE_EQ(battery1.design_capacity, 18000.0); + EXPECT_DOUBLE_EQ(battery1.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_EQ(battery1.power_supply_status, 2); // from itfs_values_[10] + EXPECT_EQ(battery1.power_supply_health, 4); // from itfs_values_[11] + EXPECT_EQ(battery1.power_supply_technology, BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO); + EXPECT_TRUE(battery1.present); // voltage > 0.0 + EXPECT_EQ(battery1.location, "slot1"); + EXPECT_EQ(battery1.serial_number, "serial_device_1"); + + // 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 battery1 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, "slot0, slot1, "); + EXPECT_EQ(battery_state_msg.serial_number, "serial_device_0, serial_device_1, "); +} + +TEST_F(BatteryStateBroadcasterTest, update_broadcasted_success) +{ + SetUpBatteryStateBroadcaster(); + + ASSERT_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); + + ASSERT_TRUE(battery0_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); + + // battery0 + const auto & battery0 = raw_battery_states_msg.battery_states[0]; + EXPECT_DOUBLE_EQ(battery0.voltage, 10.0); + // percentage calculated (no interface) = (10.0 - 0.0) * 100 / (10.0 - 0.0) = 100 + EXPECT_DOUBLE_EQ(battery0.percentage, 100.0); + EXPECT_TRUE(battery0.present); // voltage > 0.0 + + // battery1 + const auto & battery1 = raw_battery_states_msg.battery_states[1]; + EXPECT_DOUBLE_EQ(battery1.voltage, 10.0); + EXPECT_DOUBLE_EQ(battery1.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_TRUE(battery1.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_TRUE(configure_succeeds(battery_state_broadcaster_)); + ASSERT_TRUE(activate_succeeds(battery_state_broadcaster_)); + + ASSERT_TRUE(battery0_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); + + // battery0 + const auto & battery0 = raw_battery_states_msg.battery_states[0]; + EXPECT_TRUE(std::isnan(battery0.voltage)); + EXPECT_TRUE(std::isnan(battery0.percentage)); + EXPECT_FALSE(battery0.present); // voltage nan + + // battery1 + const auto & battery1 = raw_battery_states_msg.battery_states[1]; + EXPECT_DOUBLE_EQ(battery1.voltage, 10.0); + EXPECT_DOUBLE_EQ(battery1.percentage, 66.0); // directly from itfs_values_[9] + EXPECT_TRUE(battery1.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..ab22b158fc --- /dev/null +++ b/battery_state_broadcaster/test/test_battery_state_broadcaster.hpp @@ -0,0 +1,246 @@ +// 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 "controller_interface/test_utils.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/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "control_msgs/msg/battery_state_array.hpp" +#include "sensor_msgs/msg/battery_state.hpp" + +using BatteryStateMsg = sensor_msgs::msg::BatteryState; +using RawBatteryStatesMsg = control_msgs::msg::BatteryStateArray; +using controller_interface::activate_succeeds; +using controller_interface::configure_succeeds; +using controller_interface::deactivate_succeeds; +using sensor_msgs::msg::BatteryState; +using testing::IsEmpty; +using testing::SizeIs; + +// subclassing and friending so we can access member variables +class FriendBatteryStateBroadcaster : public battery_state_broadcaster::BatteryStateBroadcaster +{ + // Re-expose base class members that the tests need. + using BatteryStateBroadcaster::batteries_; + using BatteryStateBroadcaster::counts_; + using BatteryStateBroadcaster::params_; + using BatteryStateBroadcaster::sums_; + + 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(); + + battery0_voltage_itf_ = + std::make_shared("battery0", "battery_voltage"); + std::ignore = battery0_voltage_itf_->set_value(itfs_values_[0]); + battery0_temperature_itf_ = + std::make_shared("battery0", "battery_temperature"); + std::ignore = battery0_temperature_itf_->set_value(itfs_values_[1]); + battery0_charge_itf_ = + std::make_shared("battery0", "battery_charge"); + std::ignore = battery0_charge_itf_->set_value(itfs_values_[2]); + battery0_status_itf_ = std::make_shared( + "battery0", "battery_power_supply_status"); + std::ignore = battery0_status_itf_->set_value(itfs_values_[3]); + battery0_health_itf_ = std::make_shared( + "battery0", "battery_power_supply_health"); + std::ignore = battery0_health_itf_->set_value(itfs_values_[4]); + + battery1_voltage_itf_ = + std::make_shared("battery1", "battery_voltage"); + std::ignore = battery1_voltage_itf_->set_value(itfs_values_[5]); + battery1_temperature_itf_ = + std::make_shared("battery1", "battery_temperature"); + std::ignore = battery1_temperature_itf_->set_value(itfs_values_[6]); + battery1_current_itf_ = + std::make_shared("battery1", "battery_current"); + std::ignore = battery1_current_itf_->set_value(itfs_values_[7]); + battery1_charge_itf_ = + std::make_shared("battery1", "battery_charge"); + std::ignore = battery1_charge_itf_->set_value(itfs_values_[8]); + battery1_percentage_itf_ = + std::make_shared("battery1", "battery_percentage"); + std::ignore = battery1_percentage_itf_->set_value(itfs_values_[9]); + battery1_status_itf_ = std::make_shared( + "battery1", "battery_power_supply_status"); + std::ignore = battery1_status_itf_->set_value(itfs_values_[10]); + battery1_health_itf_ = std::make_shared( + "battery1", "battery_power_supply_health"); + std::ignore = battery1_health_itf_->set_value(itfs_values_[11]); + } + void TearDown() { battery_state_broadcaster_.reset(nullptr); } + + void SetUpBatteryStateBroadcaster( + const std::string controller_name = "test_battery_state_broadcaster") + { + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = battery_state_broadcaster_->define_custom_node_options(); + ASSERT_EQ(battery_state_broadcaster_->init(params), controller_interface::return_type::OK); + + std::vector state_ifs; + + state_ifs.emplace_back(battery0_voltage_itf_); + state_ifs.emplace_back(battery0_temperature_itf_); + state_ifs.emplace_back(battery0_charge_itf_); + state_ifs.emplace_back(battery0_status_itf_); + state_ifs.emplace_back(battery0_health_itf_); + + state_ifs.emplace_back(battery1_voltage_itf_); + state_ifs.emplace_back(battery1_temperature_itf_); + state_ifs.emplace_back(battery1_current_itf_); + state_ifs.emplace_back(battery1_charge_itf_); + state_ifs.emplace_back(battery1_percentage_itf_); + state_ifs.emplace_back(battery1_status_itf_); + state_ifs.emplace_back(battery1_health_itf_); + + battery_state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); + } + +protected: + // Controller-related parameters + std::vector battery_names_ = {"battery0", "battery1"}; + std::array itfs_values_ = {{ + 5.0, // 0 battery0_voltage + 60.0, // 1 battery0_temperature + 6000.0, // 2 battery0_charge + 3.0, // 3 battery0_status + 0.0, // 4 battery0_health + 10.0, // 5 battery1_voltage + 80.0, // 6 battery1_temperature + 2000.0, // 7 battery1_current + 5000.0, // 8 battery1_charge + 66.0, // 9 battery1_percentage + 2.0, // 10 battery1_status + 4.0 // 11 battery1_health + }}; + + hardware_interface::StateInterface::SharedPtr battery0_voltage_itf_; + hardware_interface::StateInterface::SharedPtr battery0_temperature_itf_; + hardware_interface::StateInterface::SharedPtr battery0_charge_itf_; + hardware_interface::StateInterface::SharedPtr battery0_status_itf_; + hardware_interface::StateInterface::SharedPtr battery0_health_itf_; + hardware_interface::StateInterface::SharedPtr battery1_voltage_itf_; + hardware_interface::StateInterface::SharedPtr battery1_temperature_itf_; + hardware_interface::StateInterface::SharedPtr battery1_current_itf_; + hardware_interface::StateInterface::SharedPtr battery1_charge_itf_; + hardware_interface::StateInterface::SharedPtr battery1_percentage_itf_; + hardware_interface::StateInterface::SharedPtr battery1_status_itf_; + hardware_interface::StateInterface::SharedPtr battery1_health_itf_; + + // 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 + rclcpp::Node test_subscription_node("test_subscription_node"); + auto raw_battery_states_subscription = + test_subscription_node.create_subscription( + "/test_battery_state_broadcaster/raw_battery_states", 10, + [](const RawBatteryStatesMsg::SharedPtr) {}); + auto battery_state_subscription = test_subscription_node.create_subscription( + "/test_battery_state_broadcaster/battery_state", 10, [](const BatteryStateMsg::SharedPtr) {}); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + RawBatteryStatesMsg received_raw_battery_states_msg; + BatteryStateMsg received_battery_state_msg; + bool has_raw_battery_states_msg = false; + bool has_battery_state_msg = false; + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(raw_battery_states_subscription); + wait_set.add_subscription(battery_state_subscription); + + int max_sub_check_loop_count = 100; // 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)); + + if (wait_set.wait(std::chrono::milliseconds(20)).kind() == rclcpp::WaitResultKind::Ready) + { + rclcpp::MessageInfo raw_msg_info; + rclcpp::MessageInfo battery_msg_info; + if (raw_battery_states_subscription->take(received_raw_battery_states_msg, raw_msg_info)) + { + has_raw_battery_states_msg = true; + } + if (battery_state_subscription->take(received_battery_state_msg, battery_msg_info)) + { + has_battery_state_msg = true; + } + } + + // check if message has been received + if (has_raw_battery_states_msg && has_battery_state_msg) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(has_raw_battery_states_msg); + ASSERT_TRUE(has_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 index 2c2009b36b..0fe90d309c 100644 --- a/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp +++ b/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 PAL Robotics SL. +// 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. @@ -24,18 +24,27 @@ TEST(TestLoadBatteryStateBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - 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_NE( - cm.load_controller( - "test_battery_state_broadcaster", "battery_state_broadcaster/BatteryStateBroadcaster"), - nullptr); + 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/doc/release_notes.rst b/doc/release_notes.rst index 97ebee6c04..4abe1cfc1c 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -5,6 +5,10 @@ Release Notes: Kilted Kaiju to Lyrical Luth This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases. +battery_state_broadcaster +************************* +* 🚀 The battery_state_broadcaster was added 🎉 (`#1888 `_). + state_interfaces_broadcaster ********************************* * 🚀 The state_interfaces_broadcaster was added 🎉 (`#2006 `_). diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3e264d806e..57a5f73914 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