add publisher for command/stateinterces in resource manager

controller_manager/src/controller_manager.cpp

@@ -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/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/single_interface_value.hpp"
 #include "hardware_interface/actuator.hpp"
 #include "hardware_interface/hardware_component_info.hpp"
 #include "hardware_interface/hardware_info.hpp"
@@ -31,6 +33,7 @@
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/time.hpp"
 
@@ -77,6 +80,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 
   ~ResourceManager();
 
+  void create_interface_value_publisher();
+
+  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.
@@ -377,6 +384,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
+  std::vector<control_msgs::msg::SingleInterfaceValue> get_all_state_interface_values() const;
+
+  void publish_all_state_interface_values() const;
+
+  std::vector<control_msgs::msg::SingleInterfaceValue> get_all_command_interface_values() const;
+
+  void publish_all_command_interface_values() const;
   /// Write all loaded hardware components.
   /**
    * Writes to all active hardware components.
@@ -410,6 +424,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   mutable std::recursive_mutex resources_lock_;
 
+  std::string interface_values_publisher_name_;
+  std::string interface_values_topic_name_;
+  rclcpp::Node::SharedPtr interface_value_publisher_node_;
+  rclcpp::Publisher<control_msgs::msg::DynamicInterfaceValues>::SharedPtr
+    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/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
 {
@@ -730,17 +731,23 @@ class ResourceStorage
 
 ResourceManager::ResourceManager(
   unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
-: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
+: interface_values_publisher_name_("resource_manager_publisher_node"),
+  interface_values_topic_name_("~/interface_values"),
+  resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+  create_interface_value_publisher();
 }
 
 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)
-: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
+: interface_values_publisher_name_("resource_manager_publisher_node"),
+  interface_values_topic_name_("~/interface_values"),
+  resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+  create_interface_value_publisher();
   load_urdf(urdf, validate_interfaces);
 
   if (activate_all)
@@ -754,6 +761,21 @@ ResourceManager::ResourceManager(
   }
 }
 
+void ResourceManager::create_interface_value_publisher()
+{
+  rclcpp::NodeOptions options;
+  interface_value_publisher_node_ =
+    rclcpp::Node::make_shared(interface_values_publisher_name_, options);
+  interface_values_publisher_ =
+    interface_value_publisher_node_->create_publisher<control_msgs::msg::DynamicInterfaceValues>(
+      interface_values_topic_name_, 10);
+}
+
+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 +1305,70 @@ HardwareReadWriteStatus ResourceManager::read(
   return read_write_status;
 }
 
+std::vector<control_msgs::msg::SingleInterfaceValue>
+ResourceManager::get_all_state_interface_values() const
+{
+  std::vector<control_msgs::msg::SingleInterfaceValue> state_interface_values;
+  for (const auto & state_interface_name : resource_storage_->available_state_interfaces_)
+  {
+    try
+    {
+      const auto state_interface_value =
+        resource_storage_->state_interface_map_.at(state_interface_name).get_value();
+      control_msgs::msg::SingleInterfaceValue interface_value;
+      interface_value.header.stamp = resource_storage_->clock_interface_->get_clock()->now();
+      interface_value.interface_name = state_interface_name;
+      interface_value.value = state_interface_value;
+      state_interface_values.push_back(interface_value);
+    }
+    catch (const std::out_of_range & e)
+    {
+      // TODO(Manuel) what to do? should probably never happen but if, is this the right place to
+      // fail?
+    }
+  }
+  return state_interface_values;
+}
+
+void ResourceManager::publish_all_state_interface_values() const
+{
+  const auto & interfaces_names_and_values = get_all_state_interface_values();
+}
+
+std::vector<control_msgs::msg::SingleInterfaceValue>
+ResourceManager::get_all_command_interface_values() const
+{
+  std::vector<control_msgs::msg::SingleInterfaceValue> command_interface_values;
+  for (const auto & command_interface_name : resource_storage_->available_command_interfaces_)
+  {
+    try
+    {
+      const auto command_interface_value =
+        resource_storage_->command_interface_map_.at(command_interface_name).get_value();
+      control_msgs::msg::SingleInterfaceValue interface_value;
+      interface_value.header.stamp = resource_storage_->clock_interface_->get_clock()->now();
+      interface_value.interface_name = command_interface_name;
+      interface_value.value = command_interface_value;
+      command_interface_values.push_back(interface_value);
+    }
+    catch (const std::out_of_range & e)
+    {
+      // TODO(Manuel) what to do? should probably never happen but if, is this the right place to
+      // fail?
+    }
+  }
+  return command_interface_values;
+}
+
+void ResourceManager::publish_all_command_interface_values() const
+{
+  control_msgs::msg::DynamicInterfaceValues interface_values;
+  interface_values.state_interface_values = get_all_state_interface_values();
+  interface_values.command_interface_values = get_all_command_interface_values();
+
+  interface_values_publisher_->publish(interface_values);
+}
+
 // CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::write(
   const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -1291,6 +1377,8 @@ HardwareReadWriteStatus ResourceManager::write(
   read_write_status.ok = true;
   read_write_status.failed_hardware_names.clear();
 
+  publish_all_command_interface_values();
+
   auto write_components = [&](auto & components)
   {
     for (auto & component : components)