feat: add mutex around step callback (#69)

.devcontainer/devcontainer.json

@@ -18,8 +18,7 @@
         "ms-vscode.cpptools-extension-pack",
         "ms-python.pylint",
         "ms-python.autopep8",
-        "ms-python.isort",
-        "eamodio.gitlens"
+        "ms-python.isort"
       ]
     }
   }

CHANGELOG.md

@@ -13,7 +13,8 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
--  chore: fix links in readme (#68)
+- chore: fix links in readme (#68)
+- feat: add mutex around step callback (#67)
 
 ## 4.0.0
 

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -58,9 +58,9 @@ class ComponentInterface {
   friend class ComponentInterfacePublicInterface;
 
   /**
-   * @brief Virtual default destructor.
+   * @brief Virtual destructor
    */
-  virtual ~ComponentInterface() = default;
+  virtual ~ComponentInterface();
 
 protected:
   /**
@@ -536,6 +536,8 @@ class ComponentInterface {
       const tf2::Duration& duration
   );
 
+  std::mutex step_mutex_; ///< Mutex for step callback
+
   std::map<std::string, utilities::PredicateVariant> predicates_; ///< Map of predicates
   std::shared_ptr<rclcpp::Publisher<modulo_component_interfaces::msg::Predicate>>
       predicate_publisher_; ///< Predicate publisher

source/modulo_components/modulo_components/component_interface.py

@@ -2,6 +2,7 @@
 import sys
 from functools import partial
 from typing import Callable, Dict, List, Optional, TypeVar, Union, Iterable
+from threading import Lock
 
 import clproto
 import modulo_core.translators.message_readers as modulo_readers
@@ -45,6 +46,7 @@ def __init__(self, node_name: str, *args, **kwargs):
         if "parameter_overrides" in node_kwargs.keys():
             node_kwargs["parameter_overrides"] = modify_parameter_overrides(node_kwargs["parameter_overrides"])
         super().__init__(node_name, *args, **node_kwargs)
+        self.__step_lock = Lock()
         self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {}
         self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {}
         self._triggers: Dict[str, bool] = {}
@@ -69,7 +71,15 @@ def __init__(self, node_name: str, *args, **kwargs):
         self._predicate_publisher = self.create_publisher(Predicate, "/predicates", self._qos)
         self.add_predicate("in_error_state", False)
 
-        self.create_timer(self.get_parameter_value("period"), self._step)
+        self.create_timer(self.get_parameter_value("period"), self.__step_with_mutex)
+
+    def __del__(self):
+        self.__step_lock.acquire()
+
+    def __step_with_mutex(self):
+        if self.__step_lock.acquire(blocking=False):
+            self._step()
+            self.__step_lock.release()
 
     def _step(self) -> None:
         """

source/modulo_components/src/ComponentInterface.cpp

@@ -37,7 +37,17 @@ ComponentInterface::ComponentInterface(
 
   this->step_timer_ = rclcpp::create_wall_timer(
       std::chrono::nanoseconds(static_cast<int64_t>(this->get_parameter_value<double>("period") * 1e9)),
-      [this] { this->step(); }, nullptr, this->node_base_.get(), this->node_timers_.get());
+      [this] {
+        if (this->step_mutex_.try_lock()) {
+          this->step();
+          this->step_mutex_.unlock();
+        }
+      },
+      nullptr, this->node_base_.get(), this->node_timers_.get());
+}
+
+ComponentInterface::~ComponentInterface() {
+  this->step_mutex_.lock();
 }
 
 void ComponentInterface::step() {}