Add interfaces_state_broadcaster

doc/release_notes.rst

@@ -4,3 +4,7 @@ Release Notes: Kilted Kaiju to Lyrical Luth
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases.
+
+interfaces_state_broadcaster
+*********************************
+* 🚀 The interfaces_state_broadcaster was added 🎉 (`#2006 <https://github.com/ros-controls/ros2_controllers/pull/2006>`_).

interfaces_state_broadcaster/CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 3.16)
+project(interfaces_state_broadcaster)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  builtin_interfaces
+  control_msgs
+  controller_interface
+  generate_parameter_library
+  pluginlib
+  rclcpp_lifecycle
+  realtime_tools
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(interfaces_state_broadcaster_parameters
+  src/interfaces_state_broadcaster_parameters.yaml
+)
+
+add_library(interfaces_state_broadcaster SHARED
+  src/interfaces_state_broadcaster.cpp
+)
+target_compile_features(interfaces_state_broadcaster PUBLIC cxx_std_17)
+target_include_directories(interfaces_state_broadcaster PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/interfaces_state_broadcaster>
+)
+target_link_libraries(interfaces_state_broadcaster PUBLIC
+                      interfaces_state_broadcaster_parameters
+                      controller_interface::controller_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${control_msgs_TARGETS}
+                      ${builtin_interfaces_TARGETS})
+pluginlib_export_plugin_description_file(controller_interface interfaces_state_broadcaster_plugin.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)
+
+  ament_add_gmock(test_load_interfaces_state_broadcaster
+    test/test_load_interfaces_state_broadcaster.cpp
+  )
+  target_link_libraries(test_load_interfaces_state_broadcaster
+    interfaces_state_broadcaster
+    controller_manager::controller_manager
+    hardware_interface::hardware_interface
+    rclcpp::rclcpp
+    ros2_control_test_assets::ros2_control_test_assets)
+  target_compile_definitions(
+    test_load_interfaces_state_broadcaster
+    PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/test_interfaces_state_broadcaster_params.yaml")
+
+  ament_add_gmock(test_interfaces_state_broadcaster
+    test/test_interfaces_state_broadcaster.cpp
+  )
+  target_link_libraries(test_interfaces_state_broadcaster
+    interfaces_state_broadcaster
+    ros2_control_test_assets::ros2_control_test_assets)
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/interfaces_state_broadcaster
+)
+install(
+  TARGETS
+    interfaces_state_broadcaster
+    interfaces_state_broadcaster_parameters
+  EXPORT export_interfaces_state_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_interfaces_state_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp

@@ -0,0 +1,85 @@
+// Copyright (c) 2025, PAL Robotics
+//
+// 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 INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_
+#define INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "control_msgs/msg/dynamic_joint_state.hpp"
+#include "control_msgs/msg/interfaces_names.hpp"
+#include "control_msgs/msg/interfaces_values.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+
+// auto-generated by generate_parameter_library
+#include "interfaces_state_broadcaster/interfaces_state_broadcaster_parameters.hpp"
+
+namespace interfaces_state_broadcaster
+{
+/**
+ * \brief Interfaces State Broadcaster for selected state interfaces in a ros2_control system.
+ *
+ * InterfacesStateBroadcaster publishes the selected state interfaces from ros2_control as ROS
+ * messages.
+ *
+ * Publishes to:
+ * - \b ~/names (control_msgs::msg::StringArray): The list of the interface names that are selected
+ * to be published by the interfaces state broadcaster. This is published with transient local
+ * durability.
+ * - \b ~/values (control_msgs::msg::ValuesArray): The list of the values corresponding to the
+ * interface names that are selected to be published by the interfaces state broadcaster.
+ *
+ * \note The values are published at the same rate as the controller update rate.
+ */
+class InterfacesStateBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  InterfacesStateBroadcaster();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) 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;
+
+protected:
+  // Optional parameters
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  // publishers and messages
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::InterfacesNames>> names_publisher_;
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::InterfacesValues>> values_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::InterfacesValues>>
+    realtime_values_publisher_;
+  control_msgs::msg::InterfacesValues values_msg_;
+  control_msgs::msg::InterfacesNames names_msg_;
+};
+
+}  // namespace interfaces_state_broadcaster
+
+#endif  // INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_

interfaces_state_broadcaster/interfaces_state_broadcaster_plugin.xml

@@ -0,0 +1,7 @@
+<library path="interfaces_state_broadcaster">
+  <class name="interfaces_state_broadcaster/InterfacesStateBroadcaster" type="interfaces_state_broadcaster::InterfacesStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The interfaces state broadcaster publishes the values of the requested interfaces from ros2_control system.
+  </description>
+  </class>
+</library>

interfaces_state_broadcaster/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>interfaces_state_broadcaster</name>
+  <version>5.8.0</version>
+  <description>Broadcaster to publish desired hardware interface states that are castable to double</description>
+
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
+  <maintainer email="christoph.froehlich@ait.ac.at">Christoph Froehlich</maintainer>
+  <maintainer email="sai.kishor@pal-robotics.com">Sai Kishor Kothakota</maintainer>
+
+  <author email="sai.kishor@pal-robotics.com">Sai Kishor Kothakota</author>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>backward_ros</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>control_msgs</depend>
+  <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp</depend>
+  <depend>realtime_tools</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp

@@ -0,0 +1,138 @@
+// Copyright (c) 2025, PAL Robotics
+//
+// 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 "interfaces_state_broadcaster/interfaces_state_broadcaster.hpp"
+
+#include <cstddef>
+#include <limits>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/time.hpp"
+
+namespace interfaces_state_broadcaster
+{
+
+InterfacesStateBroadcaster::InterfacesStateBroadcaster() {}
+
+controller_interface::CallbackReturn InterfacesStateBroadcaster::on_init()
+{
+  try
+  {
+    param_listener_ = std::make_shared<ParamListener>(get_node());
+    params_ = param_listener_->get_params();
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  return params_.interfaces.empty() ? CallbackReturn::ERROR : CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration
+InterfacesStateBroadcaster::command_interface_configuration() const
+{
+  return controller_interface::InterfaceConfiguration{
+    controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::InterfaceConfiguration
+InterfacesStateBroadcaster::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : params_.interfaces)
+  {
+    state_interfaces_config.names.push_back(interface);
+  }
+  return state_interfaces_config;
+}
+
+controller_interface::CallbackReturn InterfacesStateBroadcaster::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  params_ = param_listener_->get_params();
+
+  names_publisher_ = get_node()->create_publisher<control_msgs::msg::InterfacesNames>(
+    "~/names", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local());
+  values_publisher_ = get_node()->create_publisher<control_msgs::msg::InterfacesValues>(
+    "~/values", rclcpp::SystemDefaultsQoS());
+  realtime_values_publisher_ =
+    std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::InterfacesValues>>(
+      values_publisher_);
+
+  values_msg_.values.clear();
+  values_msg_.values.resize(params_.interfaces.size(), std::numeric_limits<double>::quiet_NaN());
+
+  names_msg_.names = params_.interfaces;
+  names_msg_.header.stamp = get_node()->now();
+  names_publisher_->publish(names_msg_);
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn InterfacesStateBroadcaster::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  for (auto i = 0u; i < state_interfaces_.size(); ++i)
+  {
+    if (!state_interfaces_[i].is_castable_to_double())
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "State interface '%s' is not castable to double. The InterfacesStateBroadcaster only "
+        "supports "
+        "state interfaces that can be casted to double.",
+        params_.interfaces[i].c_str());
+      return CallbackReturn::FAILURE;
+    }
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type InterfacesStateBroadcaster::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
+{
+  for (auto i = 0u; i < state_interfaces_.size(); ++i)
+  {
+    const auto & opt = state_interfaces_[i].get_optional();
+    if (opt.has_value())
+    {
+      values_msg_.values[i] = opt.value();
+    }
+  }
+
+  if (realtime_values_publisher_)
+  {
+    values_msg_.header.stamp = time;
+    realtime_values_publisher_->try_publish(values_msg_);
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace interfaces_state_broadcaster
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  interfaces_state_broadcaster::InterfacesStateBroadcaster,
+  controller_interface::ControllerInterface)

interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml

@@ -0,0 +1,10 @@
+interfaces_state_broadcaster:
+  interfaces: {
+    type: string_array,
+    default_value: [],
+    description: "The list of hardware interfaces information to be published by the interfaces state broadcaster.",
+    read_only: true,
+    validation: {
+      not_empty<>: []
+    }
+  }