add a bool argument spin_thread whether use callback group/executor c…

…ombo with dedicated thread to handle server

Signed-off-by: zhenpeng ge <[email protected]>

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -54,8 +54,7 @@ class SimpleActionServer
    * @param action_name Name of the action to call
    * @param execute_callback Execution  callback function of Action
    * @param server_timeout Timeout to to react to stop or preemption requests
-   * @param options Options to pass to the underlying rcl_action_server_t
-   * @param group The action server will be added to this callback group
+   * @param spin_thread  Whether to spin with a dedicated thread internally
    */
   template<typename NodeT>
   explicit SimpleActionServer(
@@ -64,14 +63,13 @@ class SimpleActionServer
     ExecuteCallback execute_callback,
     CompletionCallback completion_callback = nullptr,
     std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
-    const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+    bool spin_thread = false)
   : SimpleActionServer(
       node->get_node_base_interface(),
       node->get_node_clock_interface(),
       node->get_node_logging_interface(),
       node->get_node_waitables_interface(),
-      action_name, execute_callback, completion_callback, server_timeout, options, group)
+      action_name, execute_callback, completion_callback, server_timeout, spin_thread)
   {}
 
   /**
@@ -80,8 +78,7 @@ class SimpleActionServer
    * @param action_name Name of the action to call
    * @param execute_callback Execution  callback function of Action
    * @param server_timeout Timeout to to react to stop or preemption requests
-   * @param options Options to pass to the underlying rcl_action_server_t
-   * @param group The action server will be added to this callback group
+   * @param spin_thread  Whether to spin with a dedicated thread internally
    */
   explicit SimpleActionServer(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
@@ -92,18 +89,22 @@ class SimpleActionServer
     ExecuteCallback execute_callback,
     CompletionCallback completion_callback = nullptr,
     std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
-    const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+    bool spin_thread = false)
   : node_base_interface_(node_base_interface),
     node_clock_interface_(node_clock_interface),
     node_logging_interface_(node_logging_interface),
     node_waitables_interface_(node_waitables_interface),
     action_name_(action_name),
     execute_callback_(execute_callback),
     completion_callback_(completion_callback),
-    server_timeout_(server_timeout)
+    server_timeout_(server_timeout),
+    spin_thread_(spin_thread)
   {
     using namespace std::placeholders;  // NOLINT
+    if (spin_thread_) {
+      callback_group_ = node_base_interface->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    }
     action_server_ = rclcpp_action::create_server<ActionT>(
       node_base_interface_,
       node_clock_interface_,
@@ -113,10 +114,25 @@ class SimpleActionServer
       std::bind(&SimpleActionServer::handle_goal, this, _1, _2),
       std::bind(&SimpleActionServer::handle_cancel, this, _1),
       std::bind(&SimpleActionServer::handle_accepted, this, _1),
-      options,
-      group);
+      rcl_action_server_get_default_options(),
+      callback_group_);
+    if (spin_thread_) {
+      auto executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      executor_->add_callback_group(callback_group_, node_base_interface_);
+      executor_thread_ = std::make_unique<std::thread>([&]() {executor_->spin();});
+    }
   }
 
+  /**
+   * @brief A destructor
+   */
+  ~SimpleActionServer()
+  {
+    if (spin_thread_) {
+      executor_->cancel();
+      executor_thread_->join();
+    }
+  }
   /**
    * @brief handle the goal requested: accept or reject. This implementation always accepts.
    * @param uuid Goal ID
@@ -496,6 +512,10 @@ class SimpleActionServer
   std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> pending_handle_;
 
   typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
+  bool spin_thread_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<std::thread> executor_thread_;
 
   /**
    * @brief Generate an empty result object for an action type