diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2e703f4219..56324b323a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -267,6 +267,8 @@ ControllerManager::ControllerManager( std::make_shared>( 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>( 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."); diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index bcd03a0a16..5ebf8d23c9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + realtime_tools TinyXML2 tinyxml2_vendor ) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..eb09b744b4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -20,6 +20,8 @@ #include #include +#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::SharedPtr + interface_values_publisher_; + std::unique_ptr> + rt_interface_values_publisher_; + std::unique_ptr resource_storage_; // Structure to store read and write status so it is not initialized in the real-time loop diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index b3685a7ee2..2b7248d2c1 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -14,6 +14,7 @@ pluginlib rclcpp_lifecycle rcpputils + realtime_tools tinyxml2_vendor rcutils diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..e71e3b0fd9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/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(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(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( + "~/interface_values", 10); + rt_interface_values_publisher_ = + std::make_unique>( + 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)