diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 08ae5d0d1f..640783ecbd 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,38 +47,46 @@ 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_(); } } - 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 +108,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; @@ -135,7 +143,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; @@ -153,16 +161,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 ab2720af37..475edbf88c 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,28 +46,31 @@ 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_(); } } - 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(); } /** * @brief Get the value of the state interface. @@ -90,7 +93,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; @@ -108,16 +111,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: