From c191e8004912f2d43235d889ea20219c29b5ccc2 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Wed, 14 Feb 2024 10:24:33 +0000 Subject: [PATCH 1/5] feat: add mutex around step callback --- .../include/modulo_components/ComponentInterface.hpp | 6 ++++-- source/modulo_components/src/ComponentInterface.cpp | 11 ++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 2f9ff4f2..a03cd94f 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -58,9 +58,9 @@ class ComponentInterface { friend class ComponentInterfacePublicInterface; /** - * @brief Virtual default destructor. + * @brief Destructor */ - virtual ~ComponentInterface() = default; + ~ComponentInterface(); protected: /** @@ -536,6 +536,8 @@ class ComponentInterface { const tf2::Duration& duration ); + std::mutex step_mutex_; ///< Mutex for step callback + std::map predicates_; ///< Map of predicates std::shared_ptr> predicate_publisher_; ///< Predicate publisher diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 2f137075..431da015 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -37,7 +37,16 @@ ComponentInterface::ComponentInterface( this->step_timer_ = rclcpp::create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), - [this] { this->step(); }, nullptr, this->node_base_.get(), this->node_timers_.get()); + [this] { + std::unique_lock lock(this->step_mutex_, std::try_to_lock); + this->step(); + this->step_mutex_.unlock(); + }, + nullptr, this->node_base_.get(), this->node_timers_.get()); +} + +ComponentInterface::~ComponentInterface() { + std::lock_guard lock(this->step_mutex_); } void ComponentInterface::step() {} From ae81b20d2ccc025056f24a1eb673dd6adf480421 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Wed, 14 Feb 2024 10:28:37 +0000 Subject: [PATCH 2/5] docs: changelog --- .devcontainer/devcontainer.json | 3 +-- CHANGELOG.md | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 51ba7a8c..994ce695 100644 --- a/.devcontainer/devcontainer.json +++ b/.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" ] } } diff --git a/CHANGELOG.md b/CHANGELOG.md index 75df6b2d..8b3f2886 100644 --- a/CHANGELOG.md +++ b/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 From 5b1a420e199595f451ebf6823cb1c752f055a3c4 Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Fri, 16 Feb 2024 11:53:10 +0100 Subject: [PATCH 3/5] fix: mark desctructor virtual Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- .../include/modulo_components/ComponentInterface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index a03cd94f..0f59cd93 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -60,7 +60,7 @@ class ComponentInterface { /** * @brief Destructor */ - ~ComponentInterface(); + virtual ~ComponentInterface(); protected: /** From 19fc6b0d3b78d8a48be4a1c1ffc4e263f8bffcbf Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Mon, 19 Feb 2024 07:54:18 +0100 Subject: [PATCH 4/5] fix: docstrings --- .../include/modulo_components/ComponentInterface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 0f59cd93..56c80895 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -58,7 +58,7 @@ class ComponentInterface { friend class ComponentInterfacePublicInterface; /** - * @brief Destructor + * @brief Virtual destructor */ virtual ~ComponentInterface(); From 8e12a2a4b106d726ef3586b4407731eb0448c6eb Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 19 Feb 2024 06:53:51 +0000 Subject: [PATCH 5/5] feat: step lock for component interface --- .../modulo_components/component_interface.py | 12 +++++++++++- source/modulo_components/src/ComponentInterface.cpp | 9 +++++---- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index eb192e01..0c3dfd16 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/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: """ diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 431da015..eb4704d1 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -38,15 +38,16 @@ ComponentInterface::ComponentInterface( this->step_timer_ = rclcpp::create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), [this] { - std::unique_lock lock(this->step_mutex_, std::try_to_lock); - this->step(); - this->step_mutex_.unlock(); + if (this->step_mutex_.try_lock()) { + this->step(); + this->step_mutex_.unlock(); + } }, nullptr, this->node_base_.get(), this->node_timers_.get()); } ComponentInterface::~ComponentInterface() { - std::lock_guard lock(this->step_mutex_); + this->step_mutex_.lock(); } void ComponentInterface::step() {}