From a4e6535cf077b08f8ccd30b4a66fe69eb77c392d Mon Sep 17 00:00:00 2001 From: Guillaume Autran Date: Mon, 22 Apr 2019 15:02:45 -0400 Subject: [PATCH] Reimplement the fix a concurrency problem in the multithreaded executor Reimplement the fix to correct a concurrency problem in the multithreaded executor. This time, the AnyExecutable class has a boolean flag to reset / not reset the callback group `can_be_taken_from_` variable on destruction. The multithreaded executor initializes the executor with that flag set to false. Issue: #702 Signed-off-by: Guillaume Autran --- rclcpp/include/rclcpp/any_executable.hpp | 5 ++++- rclcpp/src/rclcpp/any_executable.cpp | 7 ++++--- rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 5 +---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 7b9bac00aa..533e1e1f15 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -35,7 +35,7 @@ namespace executor struct AnyExecutable { RCLCPP_PUBLIC - AnyExecutable(); + AnyExecutable(bool auto_reset_group = true); RCLCPP_PUBLIC virtual ~AnyExecutable(); @@ -50,6 +50,9 @@ struct AnyExecutable // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; + + // Automatically reset the callback_group `can_be_taken_from` on destruction of this object + bool auto_reset_group; }; } // namespace executor diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033bbad..eb849ac171 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -16,14 +16,15 @@ using rclcpp::executor::AnyExecutable; -AnyExecutable::AnyExecutable() +AnyExecutable::AnyExecutable(bool auto_reset_group) : subscription(nullptr), subscription_intra_process(nullptr), timer(nullptr), service(nullptr), client(nullptr), callback_group(nullptr), - node_base(nullptr) + node_base(nullptr), + auto_reset_group(auto_reset_group) {} AnyExecutable::~AnyExecutable() @@ -31,7 +32,7 @@ AnyExecutable::~AnyExecutable() // Make sure that discarded (taken but not executed) AnyExecutable's have // their callback groups reset. This can happen when an executor is canceled // between taking an AnyExecutable and executing it. - if (callback_group) { + if (auto_reset_group && callback_group) { callback_group->can_be_taken_from().store(true); } } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 24c5c79b2c..e40b76a0e4 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -71,7 +71,7 @@ void MultiThreadedExecutor::run(size_t) { while (rclcpp::ok(this->context_) && spinning.load()) { - executor::AnyExecutable any_exec; + executor::AnyExecutable any_exec(false); { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::ok(this->context_) || !spinning.load()) { @@ -101,8 +101,5 @@ MultiThreadedExecutor::run(size_t) scheduled_timers_.erase(it); } } - // Clear the callback_group to prevent the AnyExecutable destructor from - // resetting the callback group `can_be_taken_from` - any_exec.callback_group.reset(); } }