[HW interface] Resource manager publish all Command-/StateInterface values

controller_manager/src/controller_manager.cpp

@@ -258,7 +258,7 @@ ControllerManager::ControllerManager(
   const std::string & namespace_, const rclcpp::NodeOptions & options)
 : rclcpp::Node(manager_node_name, namespace_, options),
   resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
-    update_rate_, this->get_node_clock_interface())),
+    update_rate_, this->get_node_clock_interface(), get_fully_qualified_name())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -267,6 +267,8 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
+  executor_->add_node(resource_manager_->get_publisher_node());
+
   if (!get_parameter("update_rate", update_rate_))
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
@@ -308,6 +310,8 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
+  executor_->add_node(resource_manager_->get_publisher_node());
+
   if (!get_parameter("update_rate", update_rate_))
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");

hardware_interface/CMakeLists.txt

@@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
   rcpputils
   rcutils
+  realtime_tools
   TinyXML2
   tinyxml2_vendor
 )

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -20,6 +20,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "control_msgs/msg/dynamic_interface_values.hpp"
+#include "control_msgs/msg/interface_value.hpp"
 #include "hardware_interface/actuator.hpp"
 #include "hardware_interface/hardware_component_info.hpp"
 #include "hardware_interface/hardware_info.hpp"
@@ -33,6 +35,7 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/time.hpp"
+#include "realtime_tools/realtime_publisher.h"
 
 namespace hardware_interface
 {
@@ -51,7 +54,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   /// Default constructor for the Resource Manager.
   ResourceManager(
     unsigned int update_rate = 100,
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
+    const std::string & fully_qualified_ctrl_mng_node_name = "");
 
   /// Constructor for the Resource Manager.
   /**
@@ -71,12 +75,15 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   explicit ResourceManager(
     const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
     unsigned int update_rate = 100,
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
+    const std::string & fully_qualified_ctrl_mng_node_name = "");
 
   ResourceManager(const ResourceManager &) = delete;
 
   ~ResourceManager();
 
+  rclcpp::Node::SharedPtr get_publisher_node() const;
+
   /// Load resources from on a given URDF.
   /**
    * The resource manager can be post initialized with a given URDF.
@@ -400,6 +407,24 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   bool state_interface_exists(const std::string & key) const;
 
 private:
+  /**
+   * We want to publish the values of all available State-/CommandInterfaces. In order to do this we
+   * need to have a node which publishes the values. This function creates a node relative to the
+   * ControllerManager's namespace and name, as well as a RealtimePublisher for publishing the
+   * values.
+   *
+   * \param[in] fully_qualified_ctrl_mng_node_name the full qualified name of the controller manager
+   * e.g. /namespace/controller_manager_1
+   */
+  void create_interface_value_publisher(const std::string & fully_qualified_ctrl_mng_node_name);
+
+  /**
+   * Get all the values from the available State-/CommandInterface and publish them on the
+   * "~/interface_values" topic as diagnosis topic.
+   *
+   */
+  void publish_all_interface_values() const;
+
   void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
   void release_command_interface(const std::string & key);
@@ -410,6 +435,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   mutable std::recursive_mutex resources_lock_;
 
+  rclcpp::Node::SharedPtr interface_value_publisher_node_;
+  rclcpp::Publisher<control_msgs::msg::DynamicInterfaceValues>::SharedPtr
+    interface_values_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicInterfaceValues>>
+    rt_interface_values_publisher_;
+
   std::unique_ptr<ResourceStorage> resource_storage_;
 
   // Structure to store read and write status so it is not initialized in the real-time loop

hardware_interface/package.xml

@@ -14,6 +14,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
+  <depend>realtime_tools</depend>
   <depend>tinyxml2_vendor</depend>
 
   <build_depend>rcutils</build_depend>

hardware_interface/src/resource_manager.cpp

@@ -37,6 +37,7 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "rcutils/logging_macros.h"
+#include "std_msgs/msg/header.h"
 
 namespace hardware_interface
 {
@@ -729,18 +730,22 @@ class ResourceStorage
 };
 
 ResourceManager::ResourceManager(
-  unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+  const std::string & fully_qualified_ctrl_mng_node_name)
 : resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+  create_interface_value_publisher(fully_qualified_ctrl_mng_node_name);
 }
 
 ResourceManager::~ResourceManager() = default;
 
 ResourceManager::ResourceManager(
   const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate,
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+  const std::string & fully_qualified_ctrl_mng_node_name)
 : resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+  create_interface_value_publisher(fully_qualified_ctrl_mng_node_name);
   load_urdf(urdf, validate_interfaces);
 
   if (activate_all)
@@ -754,6 +759,25 @@ ResourceManager::ResourceManager(
   }
 }
 
+void ResourceManager::create_interface_value_publisher(
+  const std::string & fully_qualified_ctrl_mng_node_name)
+{
+  rclcpp::NodeOptions options;
+  interface_value_publisher_node_ = rclcpp::Node::make_shared(
+    "resource_manager_publisher_node", fully_qualified_ctrl_mng_node_name, options);
+  interface_values_publisher_ =
+    interface_value_publisher_node_->create_publisher<control_msgs::msg::DynamicInterfaceValues>(
+      "~/interface_values", 10);
+  rt_interface_values_publisher_ =
+    std::make_unique<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicInterfaceValues>>(
+      interface_values_publisher_);
+}
+
+rclcpp::Node::SharedPtr ResourceManager::get_publisher_node() const
+{
+  return interface_value_publisher_node_;
+}
+
 // CM API: Called in "callback/slow"-thread
 void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
 {
@@ -1283,6 +1307,56 @@ HardwareReadWriteStatus ResourceManager::read(
   return read_write_status;
 }
 
+void ResourceManager::publish_all_interface_values() const
+{
+  control_msgs::msg::DynamicInterfaceValues interface_values;
+  interface_values.header.stamp = resource_storage_->clock_interface_->get_clock()->now();
+
+  control_msgs::msg::InterfaceValue state_interface_values;
+  for (const auto & state_interface_name : resource_storage_->available_state_interfaces_)
+  {
+    if (const auto iter = resource_storage_->state_interface_map_.find(state_interface_name);
+        iter != resource_storage_->state_interface_map_.end())
+    {
+      state_interface_values.interface_names.push_back(state_interface_name);
+      state_interface_values.values.push_back(iter->second.get_value());
+    }
+    else
+    {
+      RCUTILS_LOG_WARN_NAMED(
+        "resource_manager",
+        "State interface '%s' is in available in state_interface_map_, but could not get the "
+        "interface",
+        state_interface_name.c_str());
+    }
+  }
+
+  control_msgs::msg::InterfaceValue command_interface_values;
+  for (const auto & command_interface_name : resource_storage_->available_command_interfaces_)
+  {
+    if (const auto iter = resource_storage_->command_interface_map_.find(command_interface_name);
+        iter != resource_storage_->command_interface_map_.end())
+    {
+      command_interface_values.interface_names.push_back(command_interface_name);
+      command_interface_values.values.push_back(iter->second.get_value());
+    }
+    else
+    {
+      RCUTILS_LOG_WARN_NAMED(
+        "resource_manager",
+        "command interface '%s' is available in command_interface_map_, but could not get the "
+        "interface",
+        command_interface_name.c_str());
+    }
+  }
+  interface_values.states = state_interface_values;
+  interface_values.commands = command_interface_values;
+
+  rt_interface_values_publisher_->trylock();
+  rt_interface_values_publisher_->msg_ = interface_values;
+  rt_interface_values_publisher_->unlockAndPublish();
+}
+
 // CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::write(
   const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -1291,6 +1365,8 @@ HardwareReadWriteStatus ResourceManager::write(
   read_write_status.ok = true;
   read_write_status.failed_hardware_names.clear();
 
+  publish_all_interface_values();
+
   auto write_components = [&](auto & components)
   {
     for (auto & component : components)