From 6fff86065110de453f0dcbf6b93e108ef4c6e9a1 Mon Sep 17 00:00:00 2001
From: TIM ANDERSON <timothyanderson096@gmail.com>
Date: Wed, 2 Sep 2026 14:54:41 +1000
Subject: [PATCH 1/2] Fix MoveItPy executor thread lifecycle

---
 .../moveit_ros/moveit_cpp/moveit_cpp.cpp      | 62 +++++++++++++++----
 1 file changed, 51 insertions(+), 11 deletions(-)

diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
index 59679ec340..c00fc4baa3 100644
--- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
+++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
@@ -35,9 +35,12 @@
 /* Author: Peter David Fagan */
 
 #include "moveit_cpp.hpp"
+#include <atomic>
+#include <chrono>
 #include <pybind11/pytypes.h>
 #include <moveit/utils/logger.hpp>
 #include <string>
+#include <thread>
 
 namespace moveit_py
 {
@@ -48,6 +51,51 @@ rclcpp::Logger getLogger()
   return moveit::getLogger("moveit.py.cpp_initializer");
 }
 
+class ExecutorThread
+{
+public:
+  explicit ExecutorThread(const rclcpp::Node::SharedPtr& node)
+    : executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>())
+    , stop_requested_(std::make_shared<std::atomic_bool>(false))
+    , execution_thread_([node, executor = executor_, stop_requested = stop_requested_]() {
+      executor->add_node(node);
+      while (!stop_requested->load())
+        executor->spin_once(std::chrono::milliseconds(100));
+    })
+  {
+  }
+
+  ~ExecutorThread()
+  {
+    stop();
+  }
+
+  ExecutorThread(const ExecutorThread&) = delete;
+  ExecutorThread& operator=(const ExecutorThread&) = delete;
+
+  void stop() noexcept
+  {
+    if (!execution_thread_.joinable())
+      return;
+
+    stop_requested_->store(true);
+    try
+    {
+      executor_->cancel();
+    }
+    catch (...)
+    {
+      // spin_once has a bounded wait, so the thread can still exit cleanly.
+    }
+    execution_thread_.join();
+  }
+
+private:
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
+  std::shared_ptr<std::atomic_bool> stop_requested_;
+  std::thread execution_thread_;
+};
+
 std::shared_ptr<moveit_cpp::PlanningComponent>
 getPlanningComponent(std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp_ptr, const std::string& planning_component)
 {
@@ -122,20 +170,12 @@ void initMoveitPy(py::module& m)
 
              RCLCPP_INFO(getLogger(), "Initialize node and executor");
              rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, name_space, node_options);
-             std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor =
-                 std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
              RCLCPP_INFO(getLogger(), "Spin separate thread");
-             auto spin_node = [node, executor]() {
-               executor->add_node(node);
-               executor->spin();
-             };
-             std::thread execution_thread(spin_node);
-             execution_thread.detach();
+             auto executor_thread = std::make_shared<ExecutorThread>(node);
 
-             auto custom_deleter = [executor](moveit_cpp::MoveItCpp* moveit_cpp) {
-               executor->cancel();
-               rclcpp::shutdown();
+             auto custom_deleter = [executor_thread](moveit_cpp::MoveItCpp* moveit_cpp) {
+               executor_thread->stop();
                delete moveit_cpp;
              };
 

From 9940ffaee383f21c03db17d2bb2bcea316426827 Mon Sep 17 00:00:00 2001
From: TIM ANDERSON <timothyanderson096@gmail.com>
Date: Wed, 2 Sep 2026 15:28:17 +1000
Subject: [PATCH 2/2] Fix MoveItPy executor self-join cleanup

---
 .../moveit_ros/moveit_cpp/moveit_cpp.cpp      | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
index c00fc4baa3..4c6f8178d2 100644
--- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
+++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
@@ -73,9 +73,14 @@ class ExecutorThread
   ExecutorThread(const ExecutorThread&) = delete;
   ExecutorThread& operator=(const ExecutorThread&) = delete;
 
+  bool isCurrentThread() const noexcept
+  {
+    return execution_thread_.joinable() && execution_thread_.get_id() == std::this_thread::get_id();
+  }
+
   void stop() noexcept
   {
-    if (!execution_thread_.joinable())
+    if (!execution_thread_.joinable() || isCurrentThread())
       return;
 
     stop_requested_->store(true);
@@ -175,6 +180,18 @@ void initMoveitPy(py::module& m)
              auto executor_thread = std::make_shared<ExecutorThread>(node);
 
              auto custom_deleter = [executor_thread](moveit_cpp::MoveItCpp* moveit_cpp) {
+               if (executor_thread->isCurrentThread())
+               {
+                 // A node callback may release the final MoveItCpp holder. Defer cleanup so the executor thread is
+                 // joined externally and MoveItCpp remains alive until the callback has returned.
+                 std::thread cleanup_thread([executor_thread, moveit_cpp]() {
+                   executor_thread->stop();
+                   delete moveit_cpp;
+                 });
+                 cleanup_thread.detach();
+                 return;
+               }
+
                executor_thread->stop();
                delete moveit_cpp;
              };