Fix MoveItPy executor thread lifecycle (#3839)

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,56 @@ 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;
+
+  bool isCurrentThread() const noexcept
+  {
+    return execution_thread_.joinable() && execution_thread_.get_id() == std::this_thread::get_id();
+  }
+
+  void stop() noexcept
+  {
+    if (!execution_thread_.joinable() || isCurrentThread())
+      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 +175,24 @@ 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_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;
+               }
 
-             auto custom_deleter = [executor](moveit_cpp::MoveItCpp* moveit_cpp) {
-               executor->cancel();
-               rclcpp::shutdown();
+               executor_thread->stop();
                delete moveit_cpp;
              };