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

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/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
 {
@@ -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,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
  */
  HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
+ void publish_all_interface_values() const;
  /// Write all loaded hardware components.
  /**
  * Writes to all active hardware components.
@@ -410,6 +418,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
 {
@@ -732,6 +733,7 @@ ResourceManager::ResourceManager(
  unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
 : resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+ create_interface_value_publisher();
 }
 
 ResourceManager::~ResourceManager() = default;
@@ -741,6 +743,7 @@ ResourceManager::ResourceManager(
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
 : resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
+ create_interface_value_publisher();
  load_urdf(urdf, validate_interfaces);
 
  if (activate_all)
@@ -754,6 +757,24 @@ ResourceManager::ResourceManager(
  }
 }
 
+void ResourceManager::create_interface_value_publisher()
+{
+ rclcpp::NodeOptions options;
+ interface_value_publisher_node_ =
+ rclcpp::Node::make_shared("resource_manager_publisher_node", 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 +1304,58 @@ 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_)
+ {
+ try
+ {
+ const auto state_interface_value =
+ resource_storage_->state_interface_map_.at(state_interface_name).get_value();
+ state_interface_values.interface_names.push_back(state_interface_name);
+ state_interface_values.values.push_back(state_interface_value);
+ }
+ catch (const std::out_of_range & e)
+ {
+ RCUTILS_LOG_WARN_NAMED(
+ "resource_manager",
+ "State interface '%s' is in available list, but could not get the interface "
+ "state_interface_map_ (std::out_of_range exception thrown).",
+ state_interface_name.c_str());
+ }
+ }
+
+ control_msgs::msg::InterfaceValue 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();
+ command_interface_values.interface_names.push_back(command_interface_name);
+ command_interface_values.values.push_back(command_interface_value);
+ }
+ catch (const std::out_of_range & e)
+ {
+ RCUTILS_LOG_WARN_NAMED(
+ "resource_manager",
+ "command interface '%s' is in available list, but could not get the interface "
+ "command_interface_map_ (std::out_of_range exception thrown).",
+ command_interface_name.c_str());
+ }
+ }
+ interface_values.states = state_interface_values;
+ interface_values.commands = command_interface_values;
+
+ rt_interface_values_publisher_->lock();
+ 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 +1364,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)