From 26d146874aabb05aa6fc504d165292047507b39d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 9 Jul 2024 13:25:57 +0200 Subject: [PATCH] Fix failing unittests: remove static executor --- core/src/introspection.cpp | 7 ++++--- core/test/test_move_relative.cpp | 3 +-- .../motion_planning_tasks/test/test_task_model.cpp | 3 +-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 2eb1d2a8..15a2509a 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -79,8 +79,9 @@ class IntrospectionExecutor { public: void add_node(const rclcpp::Node::SharedPtr& node) { - std::call_once(once_flag_, [this] { executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); }); std::lock_guard lock(mutex_); + if (!executor_) + executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); executor_->add_node(node); if (nodes_count_++ == 0) executor_thread_ = std::thread([this] { executor_->spin(); }); @@ -93,6 +94,7 @@ public: executor_->cancel(); if (executor_thread_.joinable()) executor_thread_.join(); + executor_.reset(); } } @@ -101,7 +103,6 @@ private: std::thread executor_thread_; size_t nodes_count_ = 0; std::mutex mutex_; - std::once_flag once_flag_; }; class IntrospectionPrivate @@ -157,7 +158,7 @@ public: /// services to provide an individual Solution rclcpp::Service::SharedPtr get_solution_service_; rclcpp::Node::SharedPtr node_; - inline static IntrospectionExecutor executor_; + IntrospectionExecutor executor_; /// mapping from stages to their id std::map stage_to_id_map_; diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 04133fb2..482ee4e2 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -133,6 +133,5 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw - quick_exit(RUN_ALL_TESTS()); + return RUN_ALL_TESTS(); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 9bd9d330..2e072160 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -239,6 +239,5 @@ int main(int argc, char** argv) { auto test_result = RUN_ALL_TESTS(); app.exit(test_result); }); - // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw - quick_exit(app.exec()); + return app.exec(); }