Add on_set_value_callbacks to State and Command Interface

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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/2365>`_).
 
 ros2controlcli
 **************

hardware_interface/include/hardware_interface/handle.hpp

@@ -28,6 +28,7 @@
 #include <string>
 #include <utility>
 #include <variant>
+#include <vector>
 
 #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<double(double)> on_set_value_callback)
+  {
+    std::unique_lock<std::shared_mutex> 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<std::function<double(double)>> 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_;

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<double>::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<double>(); }, 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<double(double)> amplify_callback = [](double value) -> double
+  { return value * 2.0; };
+
+  std::function<double(double)> 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<double>(); });
+  ASSERT_EQ(handle.get_optional<double>().value(), 23.0);
+  ASSERT_TRUE(handle.get_optional<double>().has_value());
+  ASSERT_TRUE(handle.set_value(0.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 0.0);
+  ASSERT_TRUE(handle.set_value(1.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 1.0);
+
+  handle.add_on_set_value_callback(amplify_callback);
+  ASSERT_TRUE(handle.set_value(2.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 4.0);
+  ASSERT_TRUE(handle.set_value(5.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 10.0);
+  ASSERT_TRUE(handle.set_value(6.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 12.0);
+  ASSERT_TRUE(handle.set_value(20.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 40.0);
+
+  handle.add_on_set_value_callback(limit_callback);
+  ASSERT_TRUE(handle.set_value(35.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 10.0);
+  ASSERT_TRUE(handle.set_value(3.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 6.0);
+  ASSERT_TRUE(handle.set_value(-45.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), -90.0);
+  ASSERT_TRUE(handle.set_value(100.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 10.0);
+
+  // Handle 2 case
+  ASSERT_TRUE(handle_2.set_value(5.0));
+  ASSERT_EQ(handle_2.get_optional<double>().value(), 5.0);
+  ASSERT_TRUE(handle_2.set_value(50.0));
+  ASSERT_EQ(handle_2.get_optional<double>().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<double>().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<double>().value(), 7.5, 1e-9);
+  ASSERT_TRUE(handle_2.set_value(20.0));
+  ASSERT_NEAR(handle_2.get_optional<double>().value(), 13.75, 1e-9);
+  ASSERT_TRUE(handle_2.set_value(100.0));
+  ASSERT_NEAR(handle_2.get_optional<double>().value(), 56.875, 1e-9);
+}
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
 TEST(TestHandle, copy_constructor)