From fafcf66d5aae912074f84ff268dbbc234f47269e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 Oct 2025 19:40:19 +0000 Subject: [PATCH 1/4] Cast all possible values --- .../include/hardware_interface/handle.hpp | 33 ++++++++++++++----- .../hardware_interface/hardware_info.hpp | 26 +++++++++++++++ 2 files changed, 50 insertions(+), 9 deletions(-) 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: From 65b7e0b0a56af2a6d9411e3cb181eba7f06d8813 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 23 Nov 2025 23:42:43 +0100 Subject: [PATCH 2/4] Fix test_actuator to use a valid pointer --- .../test/test_components/test_actuator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 83a5eeba36..ab6f7f29fd 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; } @@ -179,6 +179,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 From 26cb42166f1ee126baa979abb5f09a473fa38aa1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 23 Nov 2025 23:43:07 +0100 Subject: [PATCH 3/4] Add conditioning to skip registering invalid cases --- .../include/hardware_interface/handle.hpp | 29 +++++++++++++++++-- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index eeaa829839..e7d523fcb2 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -310,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 { @@ -346,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_; @@ -367,6 +372,15 @@ class StateInterface : public Handle void registerIntrospection() const { + 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]() @@ -386,7 +400,7 @@ class StateInterface : public Handle void unregisterIntrospection() const { - if (value_ptr_ || data_type_.is_castable_to_double()) + if (is_valid() && (value_ptr_ || data_type_.is_castable_to_double())) { DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name()); } @@ -450,6 +464,15 @@ class CommandInterface : public Handle void registerIntrospection() const { + 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]() @@ -471,7 +494,7 @@ class CommandInterface : public Handle void unregisterIntrospection() const { - if (value_ptr_ || data_type_.is_castable_to_double()) + 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( From 37de5fa3233fe3d9294a36ee3080898cc3dce214 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Nov 2025 13:45:14 +0000 Subject: [PATCH 4/4] Add unittests --- hardware_interface/test/test_handle.cpp | 48 +++++++++++++++++++++++++ 1 file changed, 48 insertions(+) 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); + } +}