diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index e7d523fcb2..53ab3baa00 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -243,6 +243,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 3d1cf89ccd..dd30c6a348 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -703,30 +703,117 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif return hardware_states_.find(interface_name) != hardware_states_.end(); } + /// Get the state interface handle + /** + * \param[in] interface_name The name of the state interface to access. + * \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. + */ + 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( + "The requested state interface not found: '{}' in hardware component: '{}'.", + interface_name, info_.name)); + } + return it->second; + } + + /// Set the value of a state interface. + /** + * \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 + bool set_state( + const StateInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) + { + 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)); + } + 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. /** * \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) { - auto it = hardware_states_.find(interface_name); - if (it == hardware_states_.end()) + 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. + * \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, when ensure_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 + { + 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)); + } + 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 + { + return interface_handle->get_value(state); } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); } /// Get the value from a state interface. @@ -740,27 +827,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif template T get_state(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."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = 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)); - } - return opt_value.value(); + T state; + get_state(get_state_interface_handle(interface_name), state); + return state; } /// Does the command interface exist? @@ -773,31 +842,116 @@ 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. + * \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 + bool set_command( + const CommandInterface::SharedPtr & interface_handle, const T & value, bool ensure_set = true) + { + 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)); + } + 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. /** * \tparam T The type of the value to be stored. * \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) { - auto it = hardware_commands_.find(interface_name); - if (it == hardware_commands_.end()) + 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. + * \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, when ensure_get is true. + */ + template + bool get_command( + const CommandInterface::SharedPtr & interface_handle, T & command, bool ensure_get = true) const + { + 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)); + } + 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 + { + return interface_handle->get_value(command); } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); } /// Get the value from a command interface. @@ -811,27 +965,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif template T get_command(const std::string & interface_name) const { - 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::shared_lock lock(handle->get_mutex()); - const auto opt_value = 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)); - } - return opt_value.value(); + T command; + get_command(get_command_interface_handle(interface_name), command); + return command; } /// Get the logger of the HardwareComponentInterface. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 7d8636350e..64d388c822 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -228,6 +228,27 @@ class HandleDataType } } + /** + * @brief Cast the given value from double. + * @param value The value to be casted (double). + * @return The casted value (std::variant). + * @throw std::runtime_error if the HandleDataType cannot be casted from double. + * @note Once we add support for more data types, this function should be updated + */ + std::variant cast_from_double(double value) const + { + switch (value_) + { + case DOUBLE: + return value; + case BOOL: + return static_cast(value); + default: + throw std::runtime_error( + fmt::format(FMT_COMPILE("Data type : '{}' cannot be casted from double."), to_string())); + } + } + HandleDataType from_string(const std::string & data_type) { return HandleDataType(data_type); } private: