new version

Author: mgonzs13

README.md

@@ -2,12 +2,11 @@
 
 This wrap uses the spin of an executor in a new thread. As a result, the calls of services and actions can be used synchronously. Besides, this node also includes predefined action servers and one client to make its use more user-friendly to developers.
 
-
 ## Examples
 
 ### Node
 
-This example shows how to create a node. To block the node, ```join_spin``` is used, which blocks the main thread till the thread of the spin ends.
+This example shows how to create a node. To block the node, `join_spin` is used, which blocks the main thread till the thread of the spin ends.
 
 ```python
 from simple_node import Node
@@ -17,13 +16,10 @@ class MyNode(Node):
     def __init__(self):
         super().__init__("my_node")
 
-def main(args=None):
-    rclpy.init(args=args)
-
+def main():
+    rclpy.init()
     node = MyNode()
-
     node.join_spin()
-
     rclpy.shutdown()
 
 

simple_node/include/simple_node/actions/action_client.hpp

@@ -16,28 +16,30 @@
 #ifndef ACTION_CLIENT_HPP
 #define ACTION_CLIENT_HPP
 
-#include <chrono>
-#include <future>
+#include <condition_variable>
 #include <memory>
 #include <string>
-#include <thread>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/client_goal_handle.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
-#include <action_msgs/msg/goal_status.hpp>
+using namespace std::placeholders;
 
 namespace simple_node {
 namespace actions {
 
 template <typename ActionT>
 class ActionClient : public rclcpp_action::Client<ActionT> {
 
-  using Goal = typename ActionT::Goal;
-  using Feedback = typename ActionT::Feedback;
+  using SendGoalOptions =
+      typename rclcpp_action::Client<ActionT>::SendGoalOptions;
   using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
+
+  using Goal = typename ActionT::Goal;
   using Result = typename GoalHandle::Result::SharedPtr;
+
+  using Feedback = typename ActionT::Feedback;
   using FeedbackCallback = std::function<void(
       typename GoalHandle::SharedPtr, const std::shared_ptr<const Feedback>)>;
 
@@ -48,172 +50,113 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
             node->get_node_base_interface(), node->get_node_graph_interface(),
             node->get_node_logging_interface(), action_name) {
 
-    this->status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN;
+    this->result = nullptr;
+    this->status = rclcpp_action::ResultCode::UNKNOWN;
 
     this->goal_handle = nullptr;
-    this->goal_thread = nullptr;
-    this->result = nullptr;
     this->feedback_cb = feedback_cb;
   }
 
-  ~ActionClient() { delete this->goal_thread; }
+  ~ActionClient() {}
 
-  void wait_for_result() { this->goal_thread->join(); }
+  void wait_for_result() {
+    std::unique_lock<std::mutex> lock(this->action_done_mutex);
+    this->action_done_cond.wait(lock);
+  }
 
   Result get_result() { return this->result; }
 
   void send_goal(Goal goal, FeedbackCallback feedback_cb = nullptr) {
 
+    this->result = nullptr;
+    this->set_status(rclcpp_action::ResultCode::UNKNOWN);
+
     FeedbackCallback _feedback_cb = this->feedback_cb;
 
     if (feedback_cb != nullptr) {
       _feedback_cb = feedback_cb;
     }
 
-    this->set_status(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
-    this->goal_thread =
-        new std::thread(&ActionClient::_send_goal, this, goal, _feedback_cb);
+    auto send_goal_options = SendGoalOptions();
+
+    send_goal_options.goal_response_callback =
+        std::bind(&ActionClient::goal_response_callback, this, _1);
+
+    send_goal_options.result_callback =
+        std::bind(&ActionClient::result_callback, this, _1);
+
+    send_goal_options.feedback_callback = _feedback_cb;
+
+    this->async_send_goal(goal, send_goal_options);
   }
 
-  bool cancel_goal() {
-    if (this->goal_handle != nullptr) {
-      return this->_cancel_goal();
-    } else {
-      return false;
+  void cancel_goal() {
+    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
+    if (this->goal_handle) {
+      this->async_cancel_goal(this->goal_handle);
     }
   }
 
   // **************
   // STATUS
   // **************
-  int8_t get_status() {
+  rclcpp_action::ResultCode get_status() {
     const std::lock_guard<std::mutex> lock(this->status_mtx);
     return this->status;
   }
 
-  bool is_accepted() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_ACCEPTED;
-  }
-
-  bool is_executing() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
-  }
-
-  bool is_canceling() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELING;
-  }
-
   bool is_succeeded() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
+    return this->get_status() == rclcpp_action::ResultCode::SUCCEEDED;
   }
 
   bool is_canceled() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELED;
+    return this->get_status() == rclcpp_action::ResultCode::CANCELED;
   }
 
   bool is_aborted() {
-    return this->get_status() == action_msgs::msg::GoalStatus::STATUS_ABORTED;
+    return this->get_status() == rclcpp_action::ResultCode::ABORTED;
   }
 
   bool is_working() {
-    return (this->is_executing() || this->is_canceling() ||
-            this->is_accepted());
+    return !this->is_terminated() &&
+           this->get_status() != rclcpp_action::ResultCode::UNKNOWN;
   }
 
   bool is_terminated() {
     return (this->is_succeeded() || this->is_canceled() || this->is_aborted());
   }
 
 private:
-  int8_t status;
-  std::mutex status_mtx;
-  std::thread *goal_thread;
+  std::condition_variable action_done_cond;
+  std::mutex action_done_mutex;
+
   Result result;
+  rclcpp_action::ResultCode status;
+  std::mutex status_mtx;
+
   std::shared_ptr<GoalHandle> goal_handle;
+  std::mutex goal_handle_mutex;
+
   FeedbackCallback feedback_cb;
 
-  void set_status(int8_t status) {
+  void set_status(rclcpp_action::ResultCode status) {
     const std::lock_guard<std::mutex> lock(this->status_mtx);
     this->status = status;
   }
 
-  void _send_goal(Goal goal, FeedbackCallback feedback_cb) {
-
-    this->result = nullptr;
-
-    auto send_goal_options =
-        typename rclcpp_action::Client<ActionT>::SendGoalOptions();
-    send_goal_options.feedback_callback = feedback_cb;
-
-    auto send_goal_future = this->async_send_goal(goal, send_goal_options);
-
-    // wait for acceptance
-    while (send_goal_future.wait_for(std::chrono::seconds(1)) !=
-           std::future_status::ready) {
-    }
-
-    // check acceptance
-    this->goal_handle = send_goal_future.get();
-    if (!this->goal_handle) {
-
-      // change status
-      if (this->is_canceled()) {
-        return;
-      }
-      this->set_status(action_msgs::msg::GoalStatus::STATUS_ABORTED);
-      return;
-    }
-
-    // change status
-    if (this->is_canceled()) {
-      return;
-    }
-    this->set_status(action_msgs::msg::GoalStatus::STATUS_ACCEPTED);
-
-    // get result
-    auto get_result_future = this->async_get_result(this->goal_handle);
-
-    // change status
-    if (this->is_canceled()) {
-      return;
-    }
-    this->set_status(action_msgs::msg::GoalStatus::STATUS_EXECUTING);
-
-    // wait for result
-    while (get_result_future.wait_for(std::chrono::seconds(1)) !=
-           std::future_status::ready) {
-    }
-
-    // change status
-    if (this->is_canceled()) {
-      return;
-    }
-
-    this->set_status((uint8_t)get_result_future.get().code);
-    this->result = get_result_future.get().result;
+  void
+  goal_response_callback(const typename GoalHandle::SharedPtr &goal_handle) {
+    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
+    this->goal_handle = goal_handle;
   }
 
-  bool _cancel_goal() {
-    int8_t old_status = this->get_status();
-
-    auto cancel_goal_future = this->async_cancel_goal(this->goal_handle);
-    this->set_status(action_msgs::msg::GoalStatus::STATUS_CANCELING);
-
-    while (cancel_goal_future.wait_for(std::chrono::seconds(1)) !=
-           std::future_status::ready) {
-    }
-
-    auto result = cancel_goal_future.get();
-    if (result->goals_canceling.size() == 0) {
-      this->set_status(old_status);
-      return false;
-    }
-
-    this->set_status(action_msgs::msg::GoalStatus::STATUS_CANCELED);
-
-    return true;
+  void result_callback(const typename GoalHandle::WrappedResult &result) {
+    this->result = result.result;
+    this->set_status(result.code);
+    this->action_done_cond.notify_one();
   }
 };
+
 } // namespace actions
 } // namespace simple_node
 #endif

simple_node/include/simple_node/actions/action_server.hpp

@@ -118,23 +118,6 @@ class ActionServer : public rclcpp_action::Server<ActionT> {
 
   void handle_execute(const std::shared_ptr<GoalHandle> goal_handle) {
 
-    (void)goal_handle;
-
-    std::thread(std::bind(&ActionServer::execute, this)).detach();
-
-    while (this->goal_handle != nullptr) {
-      if (this->goal_handle->is_canceling()) {
-
-        if (this->cancel_callback != nullptr) {
-          this->cancel_callback();
-        }
-        break;
-      }
-      std::this_thread::sleep_for(std::chrono::seconds(1));
-    }
-  }
-
-  void execute() {
     this->execute_callback(this->goal_handle);
     this->goal_handle = nullptr;
   }