Feature/battery state broadcaster

battery_state_broadcaster/CHANGELOG.rst

@@ -0,0 +1,24 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package battery_state_broadcaster
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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)
+* 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))
+* Contributors: Alejandro Hernandez Cordero, Jonas Otto
+
+1.0.1 (2025-02-06)
+------------------
+* fix realtime_tools include
+* Contributors: Jonas Otto
+
+1.0.0 (2024-08-14)
+------------------
+* initial release
+* Contributors: Jonas Otto

battery_state_broadcaster/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(battery_state_broadcaster)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  pluginlib
+  realtime_tools
+  sensor_msgs
+)
+
+# 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)
+
+add_library(battery_state_broadcaster SHARED src/BatteryStateBroadcaster.cpp)
+target_include_directories(battery_state_broadcaster PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/battery_state_broadcaster>
+)
+target_link_libraries(battery_state_broadcaster
+  PUBLIC
+    controller_interface::controller_interface
+    pluginlib::pluginlib
+    realtime_tools::realtime_tools
+    ${sensor_msgs_TARGETS}
+)
+
+pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/battery_state_broadcaster
+)
+install(
+  TARGETS
+  battery_state_broadcaster
+  EXPORT export_battery_state_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_battery_state_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()

battery_state_broadcaster/battery_state_broadcaster.xml

@@ -0,0 +1,8 @@
+<library path="battery_state_broadcaster">
+    <class name="battery_state_broadcaster/BatteryStateBroadcaster"
+           type="battery_state_broadcaster::BatteryStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
+    <description>
+        This controller publishes the readings of a battery sensor as sensor_msgs/BatteryState message.
+    </description>
+    </class>
+  </library>

battery_state_broadcaster/doc/userdoc.rst

@@ -0,0 +1,55 @@
+: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
+=========================
+This broadcaster publishes `sensor_msgs/BatteryState <https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg>`__ messages from appropriate state interfaces.
+
+
+Required State Interfaces
+---------------------------------
+This broadcaster requires the robot to have a sensor component which contains the battery state interfaces:
+
+.. code-block:: xml
+
+  <ros2_control type="system">
+
+      <!-- ... -->
+
+      <sensor name="battery_state">
+          <state_interface name="voltage" />
+      </sensor>
+  </ros2_control>
+
+Parameters
+---------------------------------
+To use this broadcaster, declare it in the controller manager and set its parameters:
+
+.. 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
+
+And spawn it in the launch file:
+
+.. code-block:: python
+
+  battery_state_broadcaster_spawner = Node(
+      package="controller_manager",
+      executable="spawner",
+      arguments=["battery_state_broadcaster"]
+
+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.

battery_state_broadcaster/include/battery_state_broadcaster/BatterySensor.hpp

@@ -0,0 +1,36 @@
+
+#include <limits>
+#include <semantic_components/semantic_component_interface.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include <sensor_msgs/msg/detail/battery_state__struct.hpp>
+
+namespace battery_state_broadcaster
+{
+class BatterySensor : public semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>
+{
+public:
+  explicit BatterySensor(const std::string& name)
+    : semantic_components::SemanticComponentInterface<sensor_msgs::msg::BatteryState>(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<float>(voltage_);
+    return true;
+  }
+
+private:
+  double voltage_ = 0.0;
+};
+}  // namespace battery_state_broadcaster

battery_state_broadcaster/include/battery_state_broadcaster/BatteryStateBroadcaster.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/publisher.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+
+#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<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_pub_;
+  std::unique_ptr<BatterySensor> battery_sensor_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>> realtime_publisher_;
+  sensor_msgs::msg::BatteryState msg_;
+};
+}  // namespace battery_state_broadcaster

battery_state_broadcaster/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>battery_state_broadcaster</name>
+  <version>1.1.0</version>
+  <description>ROS2 Control boradcaster for battery state sensors.</description>
+  <maintainer email="[email protected]">Jonas Otto</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

battery_state_broadcaster/src/BatteryStateBroadcaster.cpp

@@ -0,0 +1,101 @@
+#include "battery_state_broadcaster/BatteryStateBroadcaster.hpp"
+#include <cstdint>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/qos.hpp>
+
+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<BatterySensor>(sensor_name);
+
+  battery_state_pub_ =
+      get_node()->create_publisher<sensor_msgs::msg::BatteryState>("~/battery_state", rclcpp::SystemDefaultsQoS());
+  realtime_publisher_ =
+      std::make_unique<realtime_tools::RealtimePublisher<sensor_msgs::msg::BatteryState>>(battery_state_pub_);
+
+  msg_.temperature = std::numeric_limits<double>::quiet_NaN();
+  msg_.current = std::numeric_limits<double>::quiet_NaN();
+  msg_.charge = std::numeric_limits<double>::quiet_NaN();
+  msg_.capacity = std::numeric_limits<double>::quiet_NaN();
+  msg_.design_capacity = std::numeric_limits<double>::quiet_NaN();
+  msg_.percentage = std::numeric_limits<double>::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<float>(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)

doc/controllers_index.rst

@@ -66,6 +66,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same
    Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst>
    GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst>
    State Interfaces Broadcaster <../state_interfaces_broadcaster/doc/userdoc.rst>
+   Battery State Broadcaster <../battery_state_broadcaster/doc/userdoc.rst>
 
 Filters
 **********************