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..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 @@ -35,9 +35,12 @@ /* Author: Peter David Fagan */ #include "moveit_cpp.hpp" +#include +#include #include #include #include +#include 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()) + , stop_requested_(std::make_shared(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 executor_; + std::shared_ptr stop_requested_; + std::thread execution_thread_; +}; + std::shared_ptr getPlanningComponent(std::shared_ptr& 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 executor = - std::make_shared(); 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(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; };