feat(controllers): implement lock-free service wrappers for demanding callbacks

CHANGELOG.md

@@ -27,6 +27,7 @@ Release Versions:
 
 - fix(controllers): check for finite wrench values (#216)
 - feat(controllers): allow control type change after construction (#217)
+- feat(controllers): implement lock-free service wrappers for demanding callbacks (#218)
 
 ## 5.2.3
 

source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <any>
+#include <chrono>
 #include <mutex>
 
 #include <controller_interface/controller_interface.hpp>
@@ -310,6 +311,21 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       const std::string& service_name,
       const std::function<ControllerServiceResponse(const std::string& string)>& callback);
 
+  /**
+   * @copydetails add_service(const std::string& service_name, 
+   *                                const std::function<ControllerServiceResponse(void)>& callback)
+   */
+  void
+  add_service_lockfree(const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback);
+
+  /**
+   * @copydetails add_service(const std::string& service_name, 
+   *                                const std::function<ControllerServiceResponse(const std::string& string)>& callback)
+   */
+  void add_service_lockfree(
+      const std::string& service_name,
+      const std::function<ControllerServiceResponse(const std::string& string)>& callback);
+
   /**
    * @brief Getter of the Quality of Service attribute.
    * @return The Quality of Service attribute
@@ -439,6 +455,19 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   void add_outputs();
 
+  /** 
+   * @brief Create a service to trigger a callback function.
+   * @tparam acquire_lock If true, the command mutex is locked when invoking the callback
+   * @tparam CallbackT The type of the callback function
+   * @param service_name The name of the service
+   * @param callback A service callback function that returns a ControllerServiceResponse
+   */
+  template<bool acquire_lock, typename CallbackT>
+  void create_service(const std::string& service_name, const std::function<CallbackT>& callback)
+    requires(
+        std::is_same_v<CallbackT, ControllerServiceResponse(const std::string&)>
+        || std::is_same_v<CallbackT, ControllerServiceResponse(void)>);
+
   /**
    * @brief Validate an add_service request by parsing the service name and checking the maps of registered services.
    * @param service_name The name of the service
@@ -867,4 +896,65 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
   write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
 }
 
+template<bool acquire_lock, typename CallbackT>
+inline void
+BaseControllerInterface::create_service(const std::string& service_name, const std::function<CallbackT>& callback)
+  requires(
+      std::is_same_v<CallbackT, ControllerServiceResponse(const std::string&)>
+      || std::is_same_v<CallbackT, ControllerServiceResponse(void)>)
+{
+  constexpr bool is_empty = std::is_same_v<CallbackT, ControllerServiceResponse(void)>;
+  using ServiceT =
+      std::conditional_t<is_empty, modulo_interfaces::srv::EmptyTrigger, modulo_interfaces::srv::StringTrigger>;
+
+  auto parsed_service_name = validate_service_name(service_name, is_empty ? "empty" : "string");
+  if (parsed_service_name.empty()) {
+    return;
+  }
+
+  try {
+    auto service = get_node()->create_service<ServiceT>(
+        "~/" + parsed_service_name,
+        [this, callback](
+            const std::shared_ptr<typename ServiceT::Request> request,
+            std::shared_ptr<typename ServiceT::Response> response) {
+          try {
+            auto run_callback = [&]() {
+              ControllerServiceResponse callback_response;
+              if constexpr (is_empty) {
+                callback_response = callback();
+              } else {
+                callback_response = callback(request->payload);
+              }
+              response->success = callback_response.success;
+              response->message = callback_response.message;
+            };
+
+            if constexpr (!acquire_lock) {
+              run_callback();
+            } else {
+              std::unique_lock<std::timed_mutex> lock(this->command_mutex_, std::defer_lock);
+              if (lock.try_lock_for(std::chrono::milliseconds{100})) {
+                run_callback();
+              } else {
+                response->success = false;
+                response->message = "Unable to acquire lock for command interface within 100ms";
+              }
+            }
+          } catch (const std::exception& ex) {
+            response->success = false;
+            response->message = ex.what();
+          }
+        },
+        qos_);
+    if constexpr (is_empty) {
+      empty_services_.insert_or_assign(parsed_service_name, service);
+    } else {
+      string_services_.insert_or_assign(parsed_service_name, service);
+    }
+  } catch (const std::exception& ex) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
+  }
+}
+
 }// namespace modulo_controllers

source/modulo_controllers/src/BaseControllerInterface.cpp

@@ -1,7 +1,5 @@
 #include "modulo_controllers/BaseControllerInterface.hpp"
 
-#include <chrono>
-
 #include <lifecycle_msgs/msg/state.hpp>
 
 #include <modulo_core/translators/message_readers.hpp>
@@ -484,69 +482,24 @@ BaseControllerInterface::validate_service_name(const std::string& service_name,
 
 void BaseControllerInterface::add_service(
     const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
-  auto parsed_service_name = validate_service_name(service_name, "empty");
-  if (!parsed_service_name.empty()) {
-    try {
-      auto service = get_node()->create_service<modulo_interfaces::srv::EmptyTrigger>(
-          "~/" + parsed_service_name,
-          [this, callback](
-              const std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Request>,
-              std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Response> response) {
-            try {
-              if (this->command_mutex_.try_lock_for(100ms)) {
-                auto callback_response = callback();
-                this->command_mutex_.unlock();
-                response->success = callback_response.success;
-                response->message = callback_response.message;
-              } else {
-                response->success = false;
-                response->message = "Unable to acquire lock for command interface within 100ms";
-              }
-            } catch (const std::exception& ex) {
-              response->success = false;
-              response->message = ex.what();
-            }
-          },
-          qos_);
-      empty_services_.insert_or_assign(parsed_service_name, service);
-    } catch (const std::exception& ex) {
-      RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
-    }
-  }
+  this->create_service<true>(service_name, callback);
 }
 
 void BaseControllerInterface::add_service(
     const std::string& service_name,
     const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
-  auto parsed_service_name = validate_service_name(service_name, "string");
-  if (!parsed_service_name.empty()) {
-    try {
-      auto service = get_node()->create_service<modulo_interfaces::srv::StringTrigger>(
-          "~/" + parsed_service_name,
-          [this, callback](
-              const std::shared_ptr<modulo_interfaces::srv::StringTrigger::Request> request,
-              std::shared_ptr<modulo_interfaces::srv::StringTrigger::Response> response) {
-            try {
-              if (this->command_mutex_.try_lock_for(100ms)) {
-                auto callback_response = callback(request->payload);
-                this->command_mutex_.unlock();
-                response->success = callback_response.success;
-                response->message = callback_response.message;
-              } else {
-                response->success = false;
-                response->message = "Unable to acquire lock for command interface within 100ms";
-              }
-            } catch (const std::exception& ex) {
-              response->success = false;
-              response->message = ex.what();
-            }
-          },
-          qos_);
-      string_services_.insert_or_assign(parsed_service_name, service);
-    } catch (const std::exception& ex) {
-      RCLCPP_ERROR(get_node()->get_logger(), "Failed to add service '%s': %s", parsed_service_name.c_str(), ex.what());
-    }
-  }
+  this->create_service<true>(service_name, callback);
+}
+
+void BaseControllerInterface::add_service_lockfree(
+    const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
+  this->create_service<false>(service_name, callback);
+}
+
+void BaseControllerInterface::add_service_lockfree(
+    const std::string& service_name,
+    const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
+  this->create_service<false>(service_name, callback);
 }
 
 rclcpp::QoS BaseControllerInterface::get_qos() const {