diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 3e1e306db6..e7d523fcb2 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -28,7 +28,6 @@ #include #include #include -#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/introspection.hpp" @@ -61,8 +60,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 { @@ -313,6 +310,11 @@ class Handle /// Returns true if the handle data type can be casted to double. bool is_castable_to_double() const { return data_type_.is_castable_to_double(); } + bool is_valid() const + { + return (value_ptr_ != nullptr) || !std::holds_alternative(value_); + } + private: void copy(const Handle & other) noexcept { @@ -349,7 +351,7 @@ class Handle HandleDataType data_type_ = HandleDataType::DOUBLE; // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed - double * value_ptr_; + double * value_ptr_ = nullptr; // END mutable std::shared_mutex handle_mutex_; @@ -370,17 +372,35 @@ class StateInterface : public Handle void registerIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (!is_valid()) + { + RCLCPP_WARN( + rclcpp::get_logger(get_name()), + "Cannot register state introspection for state interface: %s without a valid value " + "pointer or initialized value.", + get_name().c_str()); + return; + } + 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 (is_valid() && (value_ptr_ || data_type_.is_castable_to_double())) { DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name()); } @@ -444,10 +464,28 @@ class CommandInterface : public Handle void registerIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (!is_valid()) + { + RCLCPP_WARN( + rclcpp::get_logger(get_name()), + "Cannot register command introspection for command interface: %s without a valid value " + "pointer or initialized value.", + get_name().c_str()); + return; + } + 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_); @@ -456,7 +494,7 @@ class CommandInterface : public Handle void unregisterIntrospection() const { - if (value_ptr_ || std::holds_alternative(value_)) + if (is_valid() && (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: diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 29dc66258b..7c1a1274a0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -426,3 +426,51 @@ TEST(TestHandle, move_assignment) EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0); } #pragma GCC diagnostic pop + +class TestableHandle : public hardware_interface::Handle +{ + FRIEND_TEST(TestHandle, handle_castable); + // Use generation of interface names + explicit TestableHandle(const InterfaceDescription & interface_description) + : hardware_interface::Handle(interface_description) + { + } +}; + +TEST(TestHandle, handle_castable) +{ + hardware_interface::InterfaceInfo info; + info.name = "position"; + const std::string JOINT_NAME_1 = "joint1"; + { + info.data_type = "double"; + info.initial_value = "23.0"; + hardware_interface::InterfaceDescription interface_description{JOINT_NAME_1, info}; + TestableHandle handle{interface_description}; + + EXPECT_TRUE(handle.is_valid()); + EXPECT_TRUE(handle.is_castable_to_double()); + EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 23.0); + } + { + info.data_type = "bool"; + info.initial_value = "false"; + hardware_interface::InterfaceDescription interface_description{JOINT_NAME_1, info}; + TestableHandle handle{interface_description}; + + EXPECT_TRUE(handle.is_valid()); + EXPECT_TRUE(handle.is_castable_to_double()); + EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 0.0); + + handle.value_ = true; + EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 1.0); + } + { + // handle with unsupported datatype can't be created right now + // extend with more datatypes once supported in Handle + hardware_interface::HandleDataType dt{"string"}; + EXPECT_FALSE(dt.is_castable_to_double()); + hardware_interface::HANDLE_DATATYPE value = std::monostate{}; + EXPECT_THROW(dt.cast_to_double(value), std::runtime_error); + } +} diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index b0aac25fea..ed44affd0a 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -75,7 +75,7 @@ class TestActuator : public ActuatorInterface &velocity_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( - get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); + get_hardware_info().joints[0].name, "some_unlisted_interface", &unlisted_interface_)); return state_interfaces; } @@ -187,6 +187,7 @@ class TestActuator : public ActuatorInterface double velocity_state_ = 0.0; double velocity_command_ = 0.0; double max_velocity_command_ = 0.0; + double unlisted_interface_ = std::numeric_limits::quiet_NaN(); }; class TestUninitializableActuator : public TestActuator