From ea4c20a816dd38b1a85fe6c6ac8e79ee2f3aac03 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 21 Dec 2023 15:39:13 +0100 Subject: [PATCH 1/5] add publisher for command/stateinterces in resource manager --- controller_manager/src/controller_manager.cpp | 4 + .../hardware_interface/resource_manager.hpp | 20 ++++ hardware_interface/src/resource_manager.cpp | 92 ++++++++++++++++++- 3 files changed, 114 insertions(+), 2 deletions(-) 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/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..8f00934aa8 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/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 get_all_state_interface_values() const; + + void publish_all_state_interface_values() const; + + std::vector 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::SharedPtr + 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/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..15bbc93b56 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 { @@ -730,8 +731,11 @@ class ResourceStorage ResourceManager::ResourceManager( unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: resource_storage_(std::make_unique(update_rate, clock_interface)) +: interface_values_publisher_name_("resource_manager_publisher_node"), + interface_values_topic_name_("~/interface_values"), + resource_storage_(std::make_unique(update_rate, clock_interface)) { + create_interface_value_publisher(); } ResourceManager::~ResourceManager() = default; @@ -739,8 +743,11 @@ 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(update_rate, clock_interface)) +: interface_values_publisher_name_("resource_manager_publisher_node"), + interface_values_topic_name_("~/interface_values"), + resource_storage_(std::make_unique(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( + 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 +ResourceManager::get_all_state_interface_values() const +{ + std::vector 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 +ResourceManager::get_all_command_interface_values() const +{ + std::vector 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) From 871649311875ac93995c4b8fb79476600068a663 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 22 Dec 2023 16:56:48 +0100 Subject: [PATCH 2/5] apply code review suggestions --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/resource_manager.hpp | 16 ++-- hardware_interface/package.xml | 1 + hardware_interface/src/resource_manager.cpp | 79 ++++++++----------- 4 files changed, 40 insertions(+), 57 deletions(-) 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 8f00934aa8..eb09b744b4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -21,7 +21,7 @@ #include #include "control_msgs/msg/dynamic_interface_values.hpp" -#include "control_msgs/msg/single_interface_value.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,9 +33,9 @@ #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" +#include "realtime_tools/realtime_publisher.h" namespace hardware_interface { @@ -384,13 +384,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); - std::vector get_all_state_interface_values() const; - - void publish_all_state_interface_values() const; - - std::vector get_all_command_interface_values() const; - - void publish_all_command_interface_values() const; + void publish_all_interface_values() const; /// Write all loaded hardware components. /** * Writes to all active hardware components. @@ -424,11 +418,11 @@ 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::SharedPtr interface_values_publisher_; + std::unique_ptr> + rt_interface_values_publisher_; std::unique_ptr resource_storage_; 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 15bbc93b56..e71e3b0fd9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -731,9 +731,7 @@ class ResourceStorage ResourceManager::ResourceManager( unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: interface_values_publisher_name_("resource_manager_publisher_node"), - interface_values_topic_name_("~/interface_values"), - resource_storage_(std::make_unique(update_rate, clock_interface)) +: resource_storage_(std::make_unique(update_rate, clock_interface)) { create_interface_value_publisher(); } @@ -743,9 +741,7 @@ 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) -: interface_values_publisher_name_("resource_manager_publisher_node"), - interface_values_topic_name_("~/interface_values"), - resource_storage_(std::make_unique(update_rate, clock_interface)) +: resource_storage_(std::make_unique(update_rate, clock_interface)) { create_interface_value_publisher(); load_urdf(urdf, validate_interfaces); @@ -765,10 +761,13 @@ void ResourceManager::create_interface_value_publisher() { rclcpp::NodeOptions options; interface_value_publisher_node_ = - rclcpp::Node::make_shared(interface_values_publisher_name_, options); + rclcpp::Node::make_shared("resource_manager_publisher_node", options); interface_values_publisher_ = interface_value_publisher_node_->create_publisher( - interface_values_topic_name_, 10); + "~/interface_values", 10); + rt_interface_values_publisher_ = + std::make_unique>( + interface_values_publisher_); } rclcpp::Node::SharedPtr ResourceManager::get_publisher_node() const @@ -1305,68 +1304,56 @@ HardwareReadWriteStatus ResourceManager::read( return read_write_status; } -std::vector -ResourceManager::get_all_state_interface_values() const +void ResourceManager::publish_all_interface_values() const { - std::vector state_interface_values; + 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(); - 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); + 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) { - // TODO(Manuel) what to do? should probably never happen but if, is this the right place to - // fail? + 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()); } } - 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 -ResourceManager::get_all_command_interface_values() const -{ - std::vector command_interface_values; + 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(); - 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); + 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) { - // TODO(Manuel) what to do? should probably never happen but if, is this the right place to - // fail? + 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()); } } - 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.states = state_interface_values; + interface_values.commands = command_interface_values; - interface_values_publisher_->publish(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 @@ -1377,7 +1364,7 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); - publish_all_command_interface_values(); + publish_all_interface_values(); auto write_components = [&](auto & components) { From 12ad220a8aceb9c14d7280a8486bba7eced99aff Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 8 Jan 2024 16:08:49 +0100 Subject: [PATCH 3/5] apply code review changes --- controller_manager/src/controller_manager.cpp | 2 +- .../hardware_interface/resource_manager.hpp | 27 +++++++++--- hardware_interface/src/resource_manager.cpp | 43 ++++++++++--------- 3 files changed, 45 insertions(+), 27 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 56324b323a..996f0c8c5d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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( - 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>( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index eb09b744b4..51b3d14db7 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -54,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. /** @@ -74,14 +75,13 @@ 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(); - void create_interface_value_publisher(); - rclcpp::Node::SharedPtr get_publisher_node() const; /// Load resources from on a given URDF. @@ -384,7 +384,6 @@ 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. @@ -408,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_info) const; void release_command_interface(const std::string & key); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e71e3b0fd9..5a3c1dcb5a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -730,20 +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(update_rate, clock_interface)) { - create_interface_value_publisher(); + 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(update_rate, clock_interface)) { - create_interface_value_publisher(); + create_interface_value_publisher(fully_qualified_ctrl_mng_node_name); load_urdf(urdf, validate_interfaces); if (activate_all) @@ -757,11 +759,12 @@ ResourceManager::ResourceManager( } } -void ResourceManager::create_interface_value_publisher() +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", 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( "~/interface_values", 10); @@ -1312,19 +1315,18 @@ void ResourceManager::publish_all_interface_values() const control_msgs::msg::InterfaceValue state_interface_values; for (const auto & state_interface_name : resource_storage_->available_state_interfaces_) { - try + if (const auto iter = resource_storage_->state_interface_map_.find(state_interface_name); + iter != resource_storage_->state_interface_map_.end()) { - 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); + state_interface_values.values.push_back(iter->second.get_value()); } - catch (const std::out_of_range & e) + else { 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 '%s' is in available in state_interface_map_, but could not get the " + "interface", state_interface_name.c_str()); } } @@ -1332,19 +1334,18 @@ void ResourceManager::publish_all_interface_values() const control_msgs::msg::InterfaceValue command_interface_values; for (const auto & command_interface_name : resource_storage_->available_command_interfaces_) { - try + if (const auto iter = resource_storage_->command_interface_map_.find(command_interface_name); + iter != resource_storage_->command_interface_map_.end()) { - 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); + command_interface_values.values.push_back(iter->second.get_value()); } - catch (const std::out_of_range & e) + else { 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 '%s' is available in command_interface_map_, but could not get the " + "interface", command_interface_name.c_str()); } } From 34ba94090220202fbbf3b90e18bf50fadab730d5 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 5 Jun 2024 14:09:34 +0200 Subject: [PATCH 4/5] use try lock instead of lock --- hardware_interface/src/resource_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5a3c1dcb5a..e51b95040c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1352,7 +1352,7 @@ void ResourceManager::publish_all_interface_values() const interface_values.states = state_interface_values; interface_values.commands = command_interface_values; - rt_interface_values_publisher_->lock(); + rt_interface_values_publisher_->trylock(); rt_interface_values_publisher_->msg_ = interface_values; rt_interface_values_publisher_->unlockAndPublish(); } From 2ef601edd52967c197ebe9663c9d2560e4d208a2 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 5 Jun 2024 14:15:00 +0200 Subject: [PATCH 5/5] forget to only publish if try lock is successfull --- hardware_interface/src/resource_manager.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e51b95040c..5eeb8509cf 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1352,9 +1352,11 @@ void ResourceManager::publish_all_interface_values() const 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(); + if(rt_interface_values_publisher_->trylock()) + { + rt_interface_values_publisher_->msg_ = interface_values; + rt_interface_values_publisher_->unlockAndPublish(); + } } // CM API: Called in "update"-thread