diff --git a/simple_node/include/simple_node/actions/action_client.hpp b/simple_node/include/simple_node/actions/action_client.hpp index 7246008..728a9ec 100644 --- a/simple_node/include/simple_node/actions/action_client.hpp +++ b/simple_node/include/simple_node/actions/action_client.hpp @@ -161,6 +161,10 @@ class ActionClient : public rclcpp_action::Client { void result_callback(const typename GoalHandle::WrappedResult &result) { this->result = result.result; this->set_status(result.code); + + std::lock_guard lock(this->goal_handle_mutex); + this->goal_handle = nullptr; + this->action_done_cond.notify_one(); } diff --git a/simple_node/include/simple_node/node.hpp b/simple_node/include/simple_node/node.hpp index 2e2fce3..ef08529 100644 --- a/simple_node/include/simple_node/node.hpp +++ b/simple_node/include/simple_node/node.hpp @@ -36,13 +36,10 @@ class Node : public rclcpp::Node { public: Node(const std::string &name, const rclcpp::NodeOptions &options = rclcpp::NodeOptions(), - rclcpp::Executor::SharedPtr executor = - std::make_shared()); + rclcpp::Executor::SharedPtr executor = nullptr); Node(const std::string &name, const std::string &namespace_, const rclcpp::NodeOptions &options = rclcpp::NodeOptions(), - rclcpp::Executor::SharedPtr executor = - std::make_shared()); - ~Node(); + rclcpp::Executor::SharedPtr executor = nullptr); void join_spin(); @@ -123,7 +120,7 @@ class Node : public rclcpp::Node { private: rclcpp::CallbackGroup::SharedPtr group; rclcpp::Executor::SharedPtr executor; - std::thread *spin_thread; + std::unique_ptr spin_thread; void run_executor(); diff --git a/simple_node/src/simple_node/node.cpp b/simple_node/src/simple_node/node.cpp index 163ed9b..e273a0a 100644 --- a/simple_node/src/simple_node/node.cpp +++ b/simple_node/src/simple_node/node.cpp @@ -26,14 +26,17 @@ Node::Node(const std::string &name, const std::string &namespace_, rclcpp::Executor::SharedPtr executor) : rclcpp::Node(name, namespace_, options), executor(executor) { + if (this->executor == nullptr) { + this->executor = + std::make_shared(); + } + rclcpp::CallbackGroup::SharedPtr group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - this->spin_thread = new std::thread(&Node::run_executor, this); + this->spin_thread = std::make_unique(&Node::run_executor, this); } -Node::~Node() { delete this->spin_thread; } - void Node::join_spin() { this->spin_thread->join(); RCLCPP_INFO(this->get_logger(), "Destroying node %s", this->get_name());