From 331a393823c50fc200c4f2d5e88251fda7fc30f6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Oct 2025 21:49:39 +0200 Subject: [PATCH 1/2] Use the pointers instead of reference in loaned interfaces --- .../loaned_command_interface.hpp | 23 +++++++++++-------- .../loaned_state_interface.hpp | 20 ++++++++-------- 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 58c6facc26..129768ede9 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -37,7 +37,7 @@ class LoanedCommandInterface } explicit LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) - : command_interface_(*command_interface), deleter_(std::forward(deleter)) + : command_interface_(command_interface), deleter_(std::forward(deleter)) { } @@ -47,7 +47,7 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { - auto logger = rclcpp::get_logger(command_interface_.get_name()); + auto logger = rclcpp::get_logger(command_interface_->get_name()); RCLCPP_WARN_EXPRESSION( rclcpp::get_logger(get_name()), (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), @@ -74,11 +74,14 @@ class LoanedCommandInterface } } - const std::string & get_name() const { return command_interface_.get_name(); } + const std::string & get_name() const { return command_interface_->get_name(); } - const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + const std::string & get_interface_name() const + { + return command_interface_->get_interface_name(); + } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return command_interface_->get_prefix_name(); } /** * @brief Set the value of the command interface. @@ -100,7 +103,7 @@ class LoanedCommandInterface { unsigned int nr_tries = 0; ++set_value_statistics_.total_counter; - while (!command_interface_.set_limited_value(value)) + while (!command_interface_->set_limited_value(value)) { ++set_value_statistics_.failed_counter; ++nr_tries; @@ -151,7 +154,7 @@ class LoanedCommandInterface do { ++get_value_statistics_.total_counter; - const std::optional data = command_interface_.get_optional(); + const std::optional data = command_interface_->get_optional(); if (data.has_value()) { return data; @@ -169,16 +172,16 @@ class LoanedCommandInterface * @brief Get the data type of the command interface. * @return The data type of the command interface. */ - HandleDataType get_data_type() const { return command_interface_.get_data_type(); } + HandleDataType get_data_type() const { return command_interface_->get_data_type(); } /** * @brief Check if the state interface can be casted to double. * @return True if the state interface can be casted to double, false otherwise. */ - bool is_castable_to_double() const { return command_interface_.is_castable_to_double(); } + bool is_castable_to_double() const { return command_interface_->is_castable_to_double(); } protected: - CommandInterface & command_interface_; + CommandInterface::SharedPtr command_interface_; Deleter deleter_; private: diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 46b087ad75..4d42908435 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -36,7 +36,7 @@ class LoanedStateInterface } explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) - : state_interface_(*state_interface), deleter_(std::forward(deleter)) + : state_interface_(state_interface), deleter_(std::forward(deleter)) { } @@ -46,13 +46,13 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { - auto logger = rclcpp::get_logger(state_interface_.get_name()); + auto logger = rclcpp::get_logger(state_interface_->get_name()); RCLCPP_WARN_EXPRESSION( logger, (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " "get_value calls", - state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, + state_interface_->get_name().c_str(), get_value_statistics_.timeout_counter, (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, get_value_statistics_.failed_counter, (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, @@ -63,11 +63,11 @@ class LoanedStateInterface } } - const std::string & get_name() const { return state_interface_.get_name(); } + const std::string & get_name() const { return state_interface_->get_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return state_interface_->get_interface_name(); } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return state_interface_->get_prefix_name(); } [[deprecated( "Use std::optional get_optional() instead to retrieve the value. This method will be " @@ -106,7 +106,7 @@ class LoanedStateInterface do { ++get_value_statistics_.total_counter; - const std::optional data = state_interface_.get_optional(); + const std::optional data = state_interface_->get_optional(); if (data.has_value()) { return data; @@ -124,16 +124,16 @@ class LoanedStateInterface * @brief Get the data type of the state interface. * @return The data type of the state interface. */ - HandleDataType get_data_type() const { return state_interface_.get_data_type(); } + HandleDataType get_data_type() const { return state_interface_->get_data_type(); } /** * @brief Check if the state interface can be casted to double. * @return True if the state interface can be casted to double, false otherwise. */ - bool is_castable_to_double() const { return state_interface_.is_castable_to_double(); } + bool is_castable_to_double() const { return state_interface_->is_castable_to_double(); } protected: - const StateInterface & state_interface_; + StateInterface::ConstSharedPtr state_interface_; Deleter deleter_; private: From e8d794a1eca3f5cf39feeb95a78e8b56befe3791 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Oct 2025 11:38:21 +0200 Subject: [PATCH 2/2] Fix the destructor --- .../loaned_command_interface.hpp | 47 ++++++++++--------- .../loaned_state_interface.hpp | 25 +++++----- 2 files changed, 40 insertions(+), 32 deletions(-) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 129768ede9..8b6f577966 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -47,27 +47,32 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { - auto logger = rclcpp::get_logger(command_interface_->get_name()); - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger(get_name()), - (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), - "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " - "%u get_value calls", - get_name().c_str(), get_value_statistics_.timeout_counter, - (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, - get_value_statistics_.failed_counter, - (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, - get_value_statistics_.total_counter); - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger(get_name()), - (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), - "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " - "%u set_value calls", - get_name().c_str(), set_value_statistics_.timeout_counter, - (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, - set_value_statistics_.failed_counter, - (set_value_statistics_.failed_counter * 100.0) / set_value_statistics_.total_counter, - set_value_statistics_.total_counter); + if (command_interface_) + { + auto logger = rclcpp::get_logger(command_interface_->get_name()); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out " + "of " + "%u get_value calls", + get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out " + "of " + "%u set_value calls", + get_name().c_str(), set_value_statistics_.timeout_counter, + (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.failed_counter, + (set_value_statistics_.failed_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); + } if (deleter_) { deleter_(); diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4d42908435..79af244d35 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -46,17 +46,20 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { - auto logger = rclcpp::get_logger(state_interface_->get_name()); - RCLCPP_WARN_EXPRESSION( - logger, - (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), - "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " - "get_value calls", - state_interface_->get_name().c_str(), get_value_statistics_.timeout_counter, - (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, - get_value_statistics_.failed_counter, - (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, - get_value_statistics_.total_counter); + if (state_interface_) + { + auto logger = rclcpp::get_logger(state_interface_->get_name()); + RCLCPP_WARN_EXPRESSION( + logger, + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " + "get_value calls", + state_interface_->get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + } if (deleter_) { deleter_();