From 3472e377cc810ef7398ae985ac067db68db121cb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 18 Nov 2025 23:24:39 +0100 Subject: [PATCH 1/6] Add get state and command interface handle methods --- .../hardware_component_interface.hpp | 193 +++++++++++++----- 1 file changed, 137 insertions(+), 56 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 3d1cf89ccd..922ef8418d 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -703,66 +703,106 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif return hardware_states_.find(interface_name) != hardware_states_.end(); } - /// Set the value of a state interface. + /// Get the state interface handle /** - * \tparam T The type of the value to be stored. * \param[in] interface_name The name of the state interface to access. - * \param[in] value The value to store. - * \throws std::runtime_error This method throws a runtime error if it cannot - * access the state interface. + * \return Shared pointer to the state interface handle. + * \throws std::runtime_error This method throws a runtime error if it cannot find the state + * interface with the given name. */ - template - void set_state(const std::string & interface_name, const T & value) + const StateInterface::SharedPtr & get_state_interface_handle( + const std::string & interface_name) const { auto it = hardware_states_.find(interface_name); if (it == hardware_states_.end()) { throw std::runtime_error( fmt::format( - FMT_COMPILE( - "State interface not found: {} in hardware component: {}. " - "This should not happen."), + "The requested state interface not found: '{}' in hardware component: '{}'.", interface_name, info_.name)); } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); + return it->second; } - /// Get the value from a state interface. + /// Set the value of a state interface. /** - * \tparam T The type of the value to be retrieved. + * \tparam T The type of the value to be stored. + * \param interface_handle The shared pointer to the state interface to access. + * \param value The value to store. + */ + template + void set_state(const StateInterface::SharedPtr & interface_handle, const T & value) + { + if (!interface_handle) + { + throw std::runtime_error( + fmt::format( + "State interface handle is null in hardware component: {}, while calling set_state " + "method. This should not happen.", + info_.name)); + } + std::unique_lock lock(interface_handle->get_mutex()); + std::ignore = interface_handle->set_value(lock, value); + } + + /// Set the value of a state interface. + /** + * \tparam T The type of the value to be stored. * \param[in] interface_name The name of the state interface to access. + * \param[in] value The value to store. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the state interface. + */ + template + void set_state(const std::string & interface_name, const T & value) + { + set_state(get_state_interface_handle(interface_name), value); + } + + /** + * \tparam T The type of the value to be retrieved. + * \param[in] interface_handle The shared pointer to the state interface to access. * \return The value obtained from the interface. * \throws std::runtime_error This method throws a runtime error if it cannot * access the state interface or its stored value. */ - template - T get_state(const std::string & interface_name) const + template + T get_state(const StateInterface::SharedPtr & interface_handle) const { - auto it = hardware_states_.find(interface_name); - if (it == hardware_states_.end()) + if (!interface_handle) { throw std::runtime_error( fmt::format( - FMT_COMPILE( - "State interface not found: {} in hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); + "State interface handle is null in hardware component: {}, while calling get_state " + "method. This should not happen.", + info_.name)); } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); + std::shared_lock lock(interface_handle->get_mutex()); + const auto opt_value = interface_handle->get_optional(lock); if (!opt_value) { throw std::runtime_error( fmt::format( - FMT_COMPILE("Failed to get state value from interface: {}. This should not happen."), - interface_name)); + "Failed to get state value from interface: {}. This should not happen.", + interface_handle->get_name())); } return opt_value.value(); } + /// Get the value from a state interface. + /** + * \tparam T The type of the value to be retrieved. + * \param[in] interface_name The name of the state interface to access. + * \return The value obtained from the interface. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the state interface or its stored value. + */ + template + T get_state(const std::string & interface_name) const + { + return get_state(get_state_interface_handle(interface_name)); + } + /// Does the command interface exist? /** * \param[in] interface_name The name of the command interface. @@ -773,6 +813,49 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif return hardware_commands_.find(interface_name) != hardware_commands_.end(); } + /// Get the command interface handle + /** + * \param[in] interface_name The name of the command interface to access. + * \return Shared pointer to the command interface handle. + * \throws std::runtime_error This method throws a runtime error if it cannot find the command + * interface with the given name. + */ + const CommandInterface::SharedPtr & get_command_interface_handle( + const std::string & interface_name) const + { + auto it = hardware_commands_.find(interface_name); + if (it == hardware_commands_.end()) + { + throw std::runtime_error( + fmt::format( + "The requested command interface not found: '{}' in hardware component: '{}'.", + interface_name, info_.name)); + } + return it->second; + } + + /// Set the value of a command interface. + /** + * \tparam T The type of the value to be stored. + * \param interface_handle The shared pointer to the command interface to access. + * \param value The value to store. + * \throws This method throws a runtime error if it cannot access the command interface. + */ + template + void set_command(const CommandInterface::SharedPtr & interface_handle, const T & value) + { + if (!interface_handle) + { + throw std::runtime_error( + fmt::format( + "Command interface handle is null in hardware component: {}, while calling set_command " + "method. This should not happen.", + info_.name)); + } + std::unique_lock lock(interface_handle->get_mutex()); + std::ignore = interface_handle->set_value(lock, value); + } + /// Set the value of a command interface. /** * \tparam T The type of the value to be stored. @@ -785,55 +868,53 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif template void set_command(const std::string & interface_name, const T & value) { - auto it = hardware_commands_.find(interface_name); - if (it == hardware_commands_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Command interface not found: {} in hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); + set_command(get_command_interface_handle(interface_name), value); } - /// Get the value from a command interface. /** * \tparam T The type of the value to be retrieved. - * \param[in] interface_name The name of the command interface to access. + * \param[in] interface_handle The shared pointer to the command interface to access. * \return The value obtained from the interface. * \throws std::runtime_error This method throws a runtime error if it cannot * access the command interface or its stored value. */ - template - T get_command(const std::string & interface_name) const + template + T get_command(const CommandInterface::SharedPtr & interface_handle) const { - auto it = hardware_commands_.find(interface_name); - if (it == hardware_commands_.end()) + if (!interface_handle) { throw std::runtime_error( fmt::format( - FMT_COMPILE( - "Command interface not found: {} in hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); + "Command interface handle is null in hardware component: {}, while calling get_command " + "method. This should not happen.", + info_.name)); } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); + std::shared_lock lock(interface_handle->get_mutex()); + const auto opt_value = interface_handle->get_optional(lock); if (!opt_value) { throw std::runtime_error( fmt::format( - FMT_COMPILE("Failed to get command value from interface: {}. This should not happen."), - interface_name)); + "Failed to get command value from interface: {}. This should not happen.", + interface_handle->get_name())); } return opt_value.value(); } + /// Get the value from a command interface. + /** + * \tparam T The type of the value to be retrieved. + * \param[in] interface_name The name of the command interface to access. + * \return The value obtained from the interface. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the command interface or its stored value. + */ + template + T get_command(const std::string & interface_name) const + { + return get_command(get_command_interface_handle(interface_name)); + } + /// Get the logger of the HardwareComponentInterface. /** * \return logger of the HardwareComponentInterface. From f3d110caa5caed0920a6fe3d7623a6cd79ed1c6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 18:05:22 +0100 Subject: [PATCH 2/6] extend methods with ensure set and get boolean flags --- .../hardware_component_interface.hpp | 139 +++++++++++++----- 1 file changed, 103 insertions(+), 36 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 922ef8418d..5581215083 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -729,9 +729,15 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param interface_handle The shared pointer to the state interface to access. * \param value The value to store. + * \param ensure_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with ensure_set = true. + * \throws std::runtime_error This method throws a runtime error if it cannot + * access the state interface. + * \return True if the value was set successfully, false otherwise. */ template - void set_state(const StateInterface::SharedPtr & interface_handle, const T & value) + bool set_state( + const StateInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) { if (!interface_handle) { @@ -741,8 +747,15 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - std::unique_lock lock(interface_handle->get_mutex()); - std::ignore = interface_handle->set_value(lock, value); + if (ensure_set) + { + std::unique_lock lock(interface_handle->get_mutex()); + return interface_handle->set_value(lock, value); + } + else + { + return interface_handle->set_value(value); + } } /// Set the value of a state interface. @@ -750,24 +763,30 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param[in] interface_name The name of the state interface to access. * \param[in] value The value to store. + * \param ensure_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with ensure_set = true. * \throws std::runtime_error This method throws a runtime error if it cannot * access the state interface. */ template - void set_state(const std::string & interface_name, const T & value) + void set_state(const std::string & interface_name, const T & value, bool ensure_set = true) { - set_state(get_state_interface_handle(interface_name), value); + std::ignore = set_state(get_state_interface_handle(interface_name), value, ensure_set); } /** * \tparam T The type of the value to be retrieved. * \param[in] interface_handle The shared pointer to the state interface to access. - * \return The value obtained from the interface. + * \param[out] state The variable to store the retrieved value. + * \param[in] ensure_get If true, the method ensures that the value is retrieved successfully + * \note This method is not real-time safe, when used with ensure_get = true. * \throws std::runtime_error This method throws a runtime error if it cannot - * access the state interface or its stored value. + * access the state interface or its stored value, when ensure_get is true. + * \return True if the value was retrieved successfully, false otherwise. */ template - T get_state(const StateInterface::SharedPtr & interface_handle) const + bool get_state( + const StateInterface::SharedPtr & interface_handle, T & state, bool ensure_get = true) const { if (!interface_handle) { @@ -777,16 +796,30 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - std::shared_lock lock(interface_handle->get_mutex()); - const auto opt_value = interface_handle->get_optional(lock); - if (!opt_value) + if (ensure_get) { - throw std::runtime_error( - fmt::format( - "Failed to get state value from interface: {}. This should not happen.", - interface_handle->get_name())); + std::shared_lock lock(interface_handle->get_mutex()); + const auto opt_value = interface_handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + "Failed to get state value from interface: {}. This should not happen.", + interface_handle->get_name())); + } + state = opt_value.value(); + return true; + } + else + { + const auto opt_value = interface_handle->get_optional(); + if (!opt_value.has_value()) + { + return false; + } + state = opt_value.value(); + return true; } - return opt_value.value(); } /// Get the value from a state interface. @@ -800,7 +833,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif template T get_state(const std::string & interface_name) const { - return get_state(get_state_interface_handle(interface_name)); + T state; + get_state(get_state_interface_handle(interface_name), state); + return state; } /// Does the command interface exist? @@ -839,10 +874,14 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param interface_handle The shared pointer to the command interface to access. * \param value The value to store. + * \param ensure_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with ensure_set = true * \throws This method throws a runtime error if it cannot access the command interface. + * \return True if the value was set successfully, false otherwise. */ template - void set_command(const CommandInterface::SharedPtr & interface_handle, const T & value) + bool set_command( + const CommandInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) { if (!interface_handle) { @@ -852,8 +891,15 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - std::unique_lock lock(interface_handle->get_mutex()); - std::ignore = interface_handle->set_value(lock, value); + if (ensure_set) + { + std::unique_lock lock(interface_handle->get_mutex()); + return interface_handle->set_value(lock, value); + } + else + { + return interface_handle->set_value(value); + } } /// Set the value of a command interface. @@ -862,24 +908,29 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \param interface_name The name of the command * interface to access. * \param value The value to store. - * \throws This method throws a runtime error if it - * cannot access the command interface. + * \param ensure_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with ensure_set = true + * \throws This method throws a runtime error if it cannot access the command interface. */ template - void set_command(const std::string & interface_name, const T & value) + void set_command(const std::string & interface_name, const T & value, bool ensure_set = true) { - set_command(get_command_interface_handle(interface_name), value); + std::ignore = set_command(get_command_interface_handle(interface_name), value, ensure_set); } /** * \tparam T The type of the value to be retrieved. * \param[in] interface_handle The shared pointer to the command interface to access. - * \return The value obtained from the interface. + * \param[out] command The variable to store the retrieved value. + * \param[in] ensure_get If true, the method ensures that the value is retrieved successfully + * \note This method is not real-time safe, when used with ensure_get = true. + * \return True if the value was retrieved successfully, false otherwise. * \throws std::runtime_error This method throws a runtime error if it cannot - * access the command interface or its stored value. + * access the command interface or its stored value, when ensure_get is true. */ template - T get_command(const CommandInterface::SharedPtr & interface_handle) const + bool get_command( + const CommandInterface::SharedPtr & interface_handle, T & command, bool ensure_get = true) const { if (!interface_handle) { @@ -889,16 +940,30 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - std::shared_lock lock(interface_handle->get_mutex()); - const auto opt_value = interface_handle->get_optional(lock); - if (!opt_value) + if (ensure_get) { - throw std::runtime_error( - fmt::format( - "Failed to get command value from interface: {}. This should not happen.", - interface_handle->get_name())); + std::shared_lock lock(interface_handle->get_mutex()); + const auto opt_value = interface_handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + "Failed to get command value from interface: {}. This should not happen.", + interface_handle->get_name())); + } + command = opt_value.value(); + return true; + } + else + { + const auto opt_value = interface_handle->get_optional(); + if (!opt_value.has_value()) + { + return false; + } + command = opt_value.value(); + return true; } - return opt_value.value(); } /// Get the value from a command interface. @@ -912,7 +977,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif template T get_command(const std::string & interface_name) const { - return get_command(get_command_interface_handle(interface_name)); + T command; + get_command(get_command_interface_handle(interface_name), command); + return command; } /// Get the logger of the HardwareComponentInterface. From 06272cf41486b1d418ea4164befecf0a43f3cb7f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 18:29:25 +0100 Subject: [PATCH 3/6] Added new get_value methods --- .../include/hardware_interface/handle.hpp | 76 +++++++++++++++++++ .../hardware_component_interface.hpp | 36 +++------ 2 files changed, 88 insertions(+), 24 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 3e1e306db6..93745e5d74 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -246,6 +246,82 @@ class Handle // END } + /** + * @brief Get the value of the handle. + * @tparam T The type of the value to be retrieved. + * @param lock The lock to access the value. + * @param value The variable to store the retrieved value. + * @return true if the value is retrieved successfully, false otherwise. + * @note The method is thread-safe and non-blocking. + * @note Ideal for the data types that are large in size to avoid copy during return. + */ + template + [[nodiscard]] bool get_value(std::shared_lock & lock, T & value) const + { + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(saikishor) get value_ if old functionality is removed + if constexpr (std::is_same_v) + { + switch (data_type_) + { + case HandleDataType::DOUBLE: + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + case HandleDataType::BOOL: + // TODO(christophfroehlich): replace with RCLCPP_WARN_ONCE once + // https://github.com/ros2/rclcpp/issues/2587 + // is fixed + if (!notified_) + { + RCLCPP_WARN( + rclcpp::get_logger(get_name()), + "Casting bool to double for interface: %s. Better use get_optional().", + get_name().c_str()); + notified_ = true; + } + value = static_cast(std::get(value_)); + return true; + default: + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Data type: '{}' cannot be casted to double for interface: {}"), + data_type_.to_string(), get_name())); + } + } + try + { + value = std::get(value_); + return true; + } + catch (const std::bad_variant_access & err) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Invalid data type: '{}' access for interface: {} expected: '{}'"), + get_type_name(), get_name(), data_type_.to_string())); + } + } + + /** + * @brief Get the value of the handle. + * @tparam T The type of the value to be retrieved. + * @param value The variable to store the retrieved value. + * @return true if the value is retrieved successfully, false otherwise. + * @note The method is thread-safe and non-blocking. + * @note Ideal for the data types that are large in size to avoid copy during return. + */ + template + [[nodiscard]] bool get_value(T & value) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + return get_value(lock, value); + } + /** * @brief Set the value of the handle. * @tparam T The type of the value to be set. diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 5581215083..7d86e518d9 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -799,26 +799,20 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif if (ensure_get) { std::shared_lock lock(interface_handle->get_mutex()); - const auto opt_value = interface_handle->get_optional(lock); - if (!opt_value) + const bool success = interface_handle->get_value(lock, state); + if (!success) { throw std::runtime_error( fmt::format( - "Failed to get state value from interface: {}. This should not happen.", - interface_handle->get_name())); + "Failed to get state value from interface: {} in hardware component: {}. This should " + "not happen", + interface_handle->get_name(), info_.name)); } - state = opt_value.value(); return true; } else { - const auto opt_value = interface_handle->get_optional(); - if (!opt_value.has_value()) - { - return false; - } - state = opt_value.value(); - return true; + return interface_handle->get_value(state); } } @@ -943,26 +937,20 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif if (ensure_get) { std::shared_lock lock(interface_handle->get_mutex()); - const auto opt_value = interface_handle->get_optional(lock); - if (!opt_value) + const bool success = interface_handle->get_value(lock, command); + if (!success) { throw std::runtime_error( fmt::format( - "Failed to get command value from interface: {}. This should not happen.", - interface_handle->get_name())); + "Failed to get command value from interface: {} in hardware component: {}. This should " + "not happen", + interface_handle->get_name(), info_.name)); } - command = opt_value.value(); return true; } else { - const auto opt_value = interface_handle->get_optional(); - if (!opt_value.has_value()) - { - return false; - } - command = opt_value.value(); - return true; + return interface_handle->get_value(command); } } From 1310cbb07c2bbaf09ee794c492c56c6c5fb126d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 19 Nov 2025 20:56:43 +0100 Subject: [PATCH 4/6] Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- .../hardware_interface/hardware_component_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 7d86e518d9..dd30c6a348 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -805,7 +805,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif throw std::runtime_error( fmt::format( "Failed to get state value from interface: {} in hardware component: {}. This should " - "not happen", + "not happen.", interface_handle->get_name(), info_.name)); } return true; @@ -943,7 +943,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif throw std::runtime_error( fmt::format( "Failed to get command value from interface: {} in hardware component: {}. This should " - "not happen", + "not happen.", interface_handle->get_name(), info_.name)); } return true; From 4ed746403c780525045d92865caf03ea19163c73 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 28 Nov 2025 00:43:45 +0100 Subject: [PATCH 5/6] Move to protected and simplify the get_value API --- .../include/hardware_interface/handle.hpp | 153 +++++++++++------- 1 file changed, 97 insertions(+), 56 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 53ab3baa00..ee211054dd 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -246,77 +246,56 @@ class Handle /** * @brief Get the value of the handle. * @tparam T The type of the value to be retrieved. - * @param lock The lock to access the value. * @param value The variable to store the retrieved value. + * @param wait_for_lock If true, the method will wait for the lock to be available, else it will + * try to get the lock without blocking. * @return true if the value is retrieved successfully, false otherwise. - * @note The method is thread-safe and non-blocking. + * + * @note The method is thread-safe. + * @note This method is real-time safe or non-blocking, only if wait_for_lock is set to false. * @note Ideal for the data types that are large in size to avoid copy during return. */ - template - [[nodiscard]] bool get_value(std::shared_lock & lock, T & value) const + template < + typename T, typename = std::enable_if_t< + !std::is_same_v, std::shared_lock>>> + [[nodiscard]] bool get_value(T & value, bool wait_for_lock) const { - if (!lock.owns_lock()) - { - return false; - } - // BEGIN (Handle export change): for backward compatibility - // TODO(saikishor) get value_ if old functionality is removed - if constexpr (std::is_same_v) - { - switch (data_type_) - { - case HandleDataType::DOUBLE: - THROW_ON_NULLPTR(value_ptr_); - value = *value_ptr_; - return true; - case HandleDataType::BOOL: - // TODO(christophfroehlich): replace with RCLCPP_WARN_ONCE once - // https://github.com/ros2/rclcpp/issues/2587 - // is fixed - if (!notified_) - { - RCLCPP_WARN( - rclcpp::get_logger(get_name()), - "Casting bool to double for interface: %s. Better use get_optional().", - get_name().c_str()); - notified_ = true; - } - value = static_cast(std::get(value_)); - return true; - default: - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Data type: '{}' cannot be casted to double for interface: {}"), - data_type_.to_string(), get_name())); - } - } - try + if (wait_for_lock) { - value = std::get(value_); - return true; + std::shared_lock lock(handle_mutex_); + return get_value(lock, value); } - catch (const std::bad_variant_access & err) + else { - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Invalid data type: '{}' access for interface: {} expected: '{}'"), - get_type_name(), get_name(), data_type_.to_string())); + std::shared_lock lock(handle_mutex_, std::try_to_lock); + return get_value(lock, value); } } /** - * @brief Get the value of the handle. - * @tparam T The type of the value to be retrieved. - * @param value The variable to store the retrieved value. - * @return true if the value is retrieved successfully, false otherwise. - * @note The method is thread-safe and non-blocking. - * @note Ideal for the data types that are large in size to avoid copy during return. + * @brief Set the value of the handle. + * @tparam T The type of the value to be set. + * @param value The value to be set. + * @param wait_for_lock If true, the method will wait for the lock to be available, else it will + * try to get the lock without blocking. + * @return true if the value is set successfully, false otherwise. + * + * @note The method is thread-safe. + * @note This method is real-time safe or non-blocking, only if wait_for_lock is set to false. */ template - [[nodiscard]] bool get_value(T & value) const + [[nodiscard]] bool set_value(const T & value, bool wait_for_lock) { - std::shared_lock lock(handle_mutex_, std::try_to_lock); - return get_value(lock, value); + if (wait_for_lock) + { + std::unique_lock lock(handle_mutex_); + return set_value(lock, value); + } + else + { + std::unique_lock lock(handle_mutex_, std::try_to_lock); + return set_value(lock, value); + } } /** @@ -391,6 +370,68 @@ class Handle return (value_ptr_ != nullptr) || !std::holds_alternative(value_); } +protected: + /** + * @brief Get the value of the handle. + * @tparam T The type of the value to be retrieved. + * @param lock The lock to access the value. + * @param value The variable to store the retrieved value. + * @return true if the value is retrieved successfully, false otherwise. + * @note The method is thread-safe and non-blocking. + * @note Ideal for the data types that are large in size to avoid copy during return. + */ + template + [[nodiscard]] bool get_value(std::shared_lock & lock, T & value) const + { + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(saikishor) get value_ if old functionality is removed + if constexpr (std::is_same_v) + { + switch (data_type_) + { + case HandleDataType::DOUBLE: + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + case HandleDataType::BOOL: + // TODO(christophfroehlich): replace with RCLCPP_WARN_ONCE once + // https://github.com/ros2/rclcpp/issues/2587 + // is fixed + if (!notified_) + { + RCLCPP_WARN( + rclcpp::get_logger(get_name()), + "Casting bool to double for interface: %s. Better use get_optional().", + get_name().c_str()); + notified_ = true; + } + value = static_cast(std::get(value_)); + return true; + default: + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Data type: '{}' cannot be casted to double for interface: {}"), + data_type_.to_string(), get_name())); + } + } + try + { + value = std::get(value_); + return true; + } + catch (const std::bad_variant_access & err) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Invalid data type: '{}' access for interface: {} expected: '{}'"), + get_type_name(), get_name(), data_type_.to_string())); + } + } + private: void copy(const Handle & other) noexcept { From a7cfdaad66055c993a9a7c537e7879bf4c72c943 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 28 Nov 2025 00:44:13 +0100 Subject: [PATCH 6/6] update the methods to use the new API and rename param as wait_for.. --- .../hardware_component_interface.hpp | 116 +++++++----------- 1 file changed, 42 insertions(+), 74 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index dd30c6a348..8fee113915 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -729,15 +729,15 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param interface_handle The shared pointer to the state interface to access. * \param value The value to store. - * \param ensure_set If true, the method ensures that the value is set successfully - * \note This method is not real-time safe, when used with ensure_set = true. + * \param wait_until_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with wait_until_set = true. * \throws std::runtime_error This method throws a runtime error if it cannot * access the state interface. * \return True if the value was set successfully, false otherwise. */ template bool set_state( - const StateInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) + const StateInterface::SharedPtr & interface_handle, const T & value, bool wait_until_set) { if (!interface_handle) { @@ -747,15 +747,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - if (ensure_set) - { - std::unique_lock lock(interface_handle->get_mutex()); - return interface_handle->set_value(lock, value); - } - else - { - return interface_handle->set_value(value); - } + return interface_handle->set_value(value, wait_until_set); } /// Set the value of a state interface. @@ -763,30 +755,29 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param[in] interface_name The name of the state interface to access. * \param[in] value The value to store. - * \param ensure_set If true, the method ensures that the value is set successfully - * \note This method is not real-time safe, when used with ensure_set = true. + * \note This method is not real-time safe. * \throws std::runtime_error This method throws a runtime error if it cannot * access the state interface. */ template - void set_state(const std::string & interface_name, const T & value, bool ensure_set = true) + void set_state(const std::string & interface_name, const T & value) { - std::ignore = set_state(get_state_interface_handle(interface_name), value, ensure_set); + std::ignore = set_state(get_state_interface_handle(interface_name), value, true); } /** * \tparam T The type of the value to be retrieved. * \param[in] interface_handle The shared pointer to the state interface to access. * \param[out] state The variable to store the retrieved value. - * \param[in] ensure_get If true, the method ensures that the value is retrieved successfully - * \note This method is not real-time safe, when used with ensure_get = true. + * \param[in] wait_until_get If true, the method ensures that the value is retrieved successfully + * \note This method is not real-time safe, when used with wait_until_get = true. * \throws std::runtime_error This method throws a runtime error if it cannot - * access the state interface or its stored value, when ensure_get is true. + * access the state interface or its stored value, when wait_until_get is true. * \return True if the value was retrieved successfully, false otherwise. */ template bool get_state( - const StateInterface::SharedPtr & interface_handle, T & state, bool ensure_get = true) const + const StateInterface::SharedPtr & interface_handle, T & state, bool wait_until_get) const { if (!interface_handle) { @@ -796,24 +787,16 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - if (ensure_get) - { - std::shared_lock lock(interface_handle->get_mutex()); - const bool success = interface_handle->get_value(lock, state); - if (!success) - { - throw std::runtime_error( - fmt::format( - "Failed to get state value from interface: {} in hardware component: {}. This should " - "not happen.", - interface_handle->get_name(), info_.name)); - } - return true; - } - else + const bool success = interface_handle->get_value(state, wait_until_get); + if (!success && wait_until_get) { - return interface_handle->get_value(state); + throw std::runtime_error( + fmt::format( + "Failed to get state value from interface: {} in hardware component: {}. This should " + "not happen.", + interface_handle->get_name(), info_.name)); } + return success; } /// Get the value from a state interface. @@ -821,6 +804,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be retrieved. * \param[in] interface_name The name of the state interface to access. * \return The value obtained from the interface. + * \note This method is not real-time safe. * \throws std::runtime_error This method throws a runtime error if it cannot * access the state interface or its stored value. */ @@ -828,7 +812,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif T get_state(const std::string & interface_name) const { T state; - get_state(get_state_interface_handle(interface_name), state); + get_state(get_state_interface_handle(interface_name), state, true); return state; } @@ -868,14 +852,14 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be stored. * \param interface_handle The shared pointer to the command interface to access. * \param value The value to store. - * \param ensure_set If true, the method ensures that the value is set successfully - * \note This method is not real-time safe, when used with ensure_set = true + * \param wait_until_set If true, the method ensures that the value is set successfully + * \note This method is not real-time safe, when used with wait_until_set = true * \throws This method throws a runtime error if it cannot access the command interface. * \return True if the value was set successfully, false otherwise. */ template bool set_command( - const CommandInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) + const CommandInterface::SharedPtr & interface_handle, const T & value, bool wait_until_set) { if (!interface_handle) { @@ -885,15 +869,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - if (ensure_set) - { - std::unique_lock lock(interface_handle->get_mutex()); - return interface_handle->set_value(lock, value); - } - else - { - return interface_handle->set_value(value); - } + return interface_handle->set_value(value, wait_until_set); } /// Set the value of a command interface. @@ -902,29 +878,28 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \param interface_name The name of the command * interface to access. * \param value The value to store. - * \param ensure_set If true, the method ensures that the value is set successfully - * \note This method is not real-time safe, when used with ensure_set = true + * \note This method is not real-time safe. * \throws This method throws a runtime error if it cannot access the command interface. */ template - void set_command(const std::string & interface_name, const T & value, bool ensure_set = true) + void set_command(const std::string & interface_name, const T & value) { - std::ignore = set_command(get_command_interface_handle(interface_name), value, ensure_set); + std::ignore = set_command(get_command_interface_handle(interface_name), value, true); } /** * \tparam T The type of the value to be retrieved. * \param[in] interface_handle The shared pointer to the command interface to access. * \param[out] command The variable to store the retrieved value. - * \param[in] ensure_get If true, the method ensures that the value is retrieved successfully - * \note This method is not real-time safe, when used with ensure_get = true. + * \param[in] wait_until_get If true, the method ensures that the value is retrieved successfully + * \note This method is not real-time safe, when used with wait_until_get = true. * \return True if the value was retrieved successfully, false otherwise. * \throws std::runtime_error This method throws a runtime error if it cannot - * access the command interface or its stored value, when ensure_get is true. + * access the command interface or its stored value, when wait_until_get is true. */ template bool get_command( - const CommandInterface::SharedPtr & interface_handle, T & command, bool ensure_get = true) const + const CommandInterface::SharedPtr & interface_handle, T & command, bool wait_until_get) const { if (!interface_handle) { @@ -934,24 +909,16 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif "method. This should not happen.", info_.name)); } - if (ensure_get) - { - std::shared_lock lock(interface_handle->get_mutex()); - const bool success = interface_handle->get_value(lock, command); - if (!success) - { - throw std::runtime_error( - fmt::format( - "Failed to get command value from interface: {} in hardware component: {}. This should " - "not happen.", - interface_handle->get_name(), info_.name)); - } - return true; - } - else + const bool success = interface_handle->get_value(command, wait_until_get); + if (!success && wait_until_get) { - return interface_handle->get_value(command); + throw std::runtime_error( + fmt::format( + "Failed to get command value from interface: {} in hardware component: {}. This should " + "not happen.", + interface_handle->get_name(), info_.name)); } + return success; } /// Get the value from a command interface. @@ -959,6 +926,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \tparam T The type of the value to be retrieved. * \param[in] interface_name The name of the command interface to access. * \return The value obtained from the interface. + * \note This method is not real-time safe. * \throws std::runtime_error This method throws a runtime error if it cannot * access the command interface or its stored value. */ @@ -966,7 +934,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif T get_command(const std::string & interface_name) const { T command; - get_command(get_command_interface_handle(interface_name), command); + get_command(get_command_interface_handle(interface_name), command, true); return command; }