diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 02a869d7b8..af9231153d 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -27,7 +27,6 @@ #include #include #include -#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/introspection.hpp" @@ -60,8 +59,6 @@ std::string get_type_name() namespace hardware_interface { -using HANDLE_DATATYPE = std::variant; - /// A handle used to get and set a value on a given interface. class Handle { @@ -357,17 +354,26 @@ class StateInterface : public Handle void registerIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (value_ptr_ || data_type_.is_castable_to_double()) { std::function f = [this]() - { return value_ptr_ ? *value_ptr_ : std::get(value_); }; + { + if (value_ptr_) + { + return *value_ptr_; + } + else + { + return data_type_.cast_to_double(value_); + } + }; DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f); } } void unregisterIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (value_ptr_ || data_type_.is_castable_to_double()) { DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name()); } @@ -431,10 +437,19 @@ class CommandInterface : public Handle void registerIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (value_ptr_ || data_type_.is_castable_to_double()) { std::function f = [this]() - { return value_ptr_ ? *value_ptr_ : std::get(value_); }; + { + if (value_ptr_) + { + return *value_ptr_; + } + else + { + return data_type_.cast_to_double(value_); + } + }; DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f); DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION( "command_interface." + get_name() + ".is_limited", &is_command_limited_); @@ -443,7 +458,7 @@ class CommandInterface : public Handle void unregisterIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (value_ptr_ || data_type_.is_castable_to_double()) { DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name()); DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION( diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 013e20a07e..719c222a65 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,8 +15,11 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include + #include #include +#include #include #include "joint_limits/joint_limits.hpp" @@ -133,6 +136,8 @@ struct TransmissionInfo /** * Hardware handles supported types */ + +using HANDLE_DATATYPE = std::variant; class HandleDataType { public: @@ -202,6 +207,27 @@ class HandleDataType } } + /** + * @brief Cast the given value to double. + * @param value The value to be casted. + * @return The casted value. + * @throw std::runtime_error if the HandleDataType cannot be casted to double. + * @note Once we add support for more data types, this function should be updated + */ + double cast_to_double(const HANDLE_DATATYPE & value) const + { + switch (value_) + { + case DOUBLE: + return std::get(value); + case BOOL: + return static_cast(std::get(value)); + default: + throw std::runtime_error( + fmt::format(FMT_COMPILE("Data type : '{}' cannot be casted to double."), to_string())); + } + } + HandleDataType from_string(const std::string & data_type) { return HandleDataType(data_type); } private: