diff --git a/doc/release_notes.rst b/doc/release_notes.rst index af55ac5bb5..3f99f29d35 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -13,6 +13,7 @@ controller_manager hardware_interface ****************** * The ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` methods will now only receive the start/stop interfaces that belong to the hardware component instead of everything (`#2120 `_) +* The ``StateInterface`` and ``CommandInterface`` classes now have a ``add_on_set_value_callback`` method that allows to register a callback that will be called when the value of the interface is set. The resulting value after passing through all the callbacks will be the one set in the interface (`#2365 `_). ros2controlcli ************** diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 59d66468e6..d5cb3a3bad 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/introspection.hpp" @@ -283,7 +284,12 @@ class Handle { // If the template is of type double, check if the value_ptr_ is not nullptr THROW_ON_NULLPTR(value_ptr_); - *value_ptr_ = value; + double new_value = value; + for (const auto & callback : on_set_value_callbacks_) + { + new_value = callback(new_value); + } + *value_ptr_ = new_value; } else { @@ -300,6 +306,25 @@ class Handle // END } + /** + * @brief Add a callback to be called when the value is set. + * @param on_set_value_callback The callback to be added. + * @note This callback is only valid for the Handle with data type double. + */ + void add_on_set_value_callback(std::function on_set_value_callback) + { + std::unique_lock lock(handle_mutex_); + if (data_type_ != HandleDataType::DOUBLE) + { + throw std::runtime_error( + fmt::format( + "Invalid data type : '{}' for interface : {}. Only double type supports on_set_value " + "callbacks.", + data_type_.to_string(), get_name())); + } + on_set_value_callbacks_.emplace_back(std::move(on_set_value_callback)); + } + std::shared_mutex & get_mutex() const { return handle_mutex_; } HandleDataType get_data_type() const { return data_type_; } @@ -338,6 +363,7 @@ class Handle std::string handle_name_; HANDLE_DATATYPE value_ = std::monostate{}; HandleDataType data_type_ = HandleDataType::DOUBLE; + std::vector> on_set_value_callbacks_; // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 67d8f827ae..14d3570572 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -30,6 +30,43 @@ namespace { constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; + +class InterfaceSetValueFilter +{ +public: + InterfaceSetValueFilter() = default; + + double amplifier(double value) + { + std::cout << "Amplifying value: " << value << std::endl; + return value * 2.0; // Amplify the value by a factor of 2 + } + + double attenuator(double value) + { + std::cout << "Attenuating value: " << value << std::endl; + return value / 2.0; // Attenuate the value by a factor of 2 + } + + double low_pass_filter(double value) + { + std::cout << "Applying low-pass filter to value: " << value << std::endl; + // Simple low-pass filter implementation + if (std::isfinite(last_value_)) + { + const double alpha = 0.5; // Smoothing factor + last_value_ = alpha * value + (1.0 - alpha) * last_value_; + } + else + { + last_value_ = value; // Initialize with the first value + } + return last_value_; + } + +private: + double last_value_ = std::numeric_limits::quiet_NaN(); +}; } // namespace #pragma GCC diagnostic push @@ -281,6 +318,10 @@ TEST(TestHandle, handle_constructor_bool_data_type) ASSERT_THROW({ std::ignore = handle.set_value(-1.0); }, std::runtime_error); ASSERT_THROW({ std::ignore = handle.set_value(0.0); }, std::runtime_error); ASSERT_THROW({ std::ignore = handle.get_optional(); }, std::runtime_error); + ASSERT_THROW( + { handle.add_on_set_value_callback([](double value) -> double { return value * 2.0; }); }, + std::runtime_error) + << "Bool data type should not support add_on_set_value_callback"; } TEST(TestHandle, interface_description_unknown_data_type) @@ -311,6 +352,83 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); } + +TEST(TestHandle, handle_double_data_type_add_on_set_value_callbacks) +{ + const std::string position_interface = "position"; + const std::string velocity_interface = "velocity"; + const std::string joint_name_1 = "joint1"; + StateInterface handle{joint_name_1, position_interface, "double", "23.0"}; + CommandInterface handle_2{joint_name_1, velocity_interface, "double", "2.0"}; + + std::function amplify_callback = [](double value) -> double + { return value * 2.0; }; + + std::function limit_callback = [](double value) -> double + { + if (value > 10.0) + { + return 10.0; + } + return value; + }; + + ASSERT_EQ(hardware_interface::HandleDataType::DOUBLE, handle.get_data_type()); + EXPECT_EQ(handle.get_name(), joint_name_1 + "/" + position_interface); + EXPECT_EQ(handle.get_interface_name(), position_interface); + EXPECT_EQ(handle.get_prefix_name(), joint_name_1); + EXPECT_NO_THROW({ std::ignore = handle.get_optional(); }); + ASSERT_EQ(handle.get_optional().value(), 23.0); + ASSERT_TRUE(handle.get_optional().has_value()); + ASSERT_TRUE(handle.set_value(0.0)); + ASSERT_EQ(handle.get_optional().value(), 0.0); + ASSERT_TRUE(handle.set_value(1.0)); + ASSERT_EQ(handle.get_optional().value(), 1.0); + + handle.add_on_set_value_callback(amplify_callback); + ASSERT_TRUE(handle.set_value(2.0)); + ASSERT_EQ(handle.get_optional().value(), 4.0); + ASSERT_TRUE(handle.set_value(5.0)); + ASSERT_EQ(handle.get_optional().value(), 10.0); + ASSERT_TRUE(handle.set_value(6.0)); + ASSERT_EQ(handle.get_optional().value(), 12.0); + ASSERT_TRUE(handle.set_value(20.0)); + ASSERT_EQ(handle.get_optional().value(), 40.0); + + handle.add_on_set_value_callback(limit_callback); + ASSERT_TRUE(handle.set_value(35.0)); + ASSERT_EQ(handle.get_optional().value(), 10.0); + ASSERT_TRUE(handle.set_value(3.0)); + ASSERT_EQ(handle.get_optional().value(), 6.0); + ASSERT_TRUE(handle.set_value(-45.0)); + ASSERT_EQ(handle.get_optional().value(), -90.0); + ASSERT_TRUE(handle.set_value(100.0)); + ASSERT_EQ(handle.get_optional().value(), 10.0); + + // Handle 2 case + ASSERT_TRUE(handle_2.set_value(5.0)); + ASSERT_EQ(handle_2.get_optional().value(), 5.0); + ASSERT_TRUE(handle_2.set_value(50.0)); + ASSERT_EQ(handle_2.get_optional().value(), 50.0); + // Add cascaded operation to amplify, filter and then attenuate the value + InterfaceSetValueFilter filter; + handle_2.add_on_set_value_callback( + std::bind(&InterfaceSetValueFilter::amplifier, &filter, std::placeholders::_1)); + handle_2.add_on_set_value_callback( + std::bind(&InterfaceSetValueFilter::low_pass_filter, &filter, std::placeholders::_1)); + handle_2.add_on_set_value_callback( + std::bind(&InterfaceSetValueFilter::attenuator, &filter, std::placeholders::_1)); + + ASSERT_TRUE(handle_2.set_value(5.0)); + ASSERT_NEAR(handle_2.get_optional().value(), 5.0, 1e-9) << "As this is the first value"; + ASSERT_TRUE(handle_2.set_value(10.0)); + ASSERT_NEAR(handle_2.get_optional().value(), 7.5, 1e-9); + ASSERT_TRUE(handle_2.set_value(20.0)); + ASSERT_NEAR(handle_2.get_optional().value(), 13.75, 1e-9); + ASSERT_TRUE(handle_2.set_value(100.0)); + ASSERT_NEAR(handle_2.get_optional().value(), 56.875, 1e-9); +} + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST(TestHandle, copy_constructor)