add payloads to Action (#66)

Author: facontidavide

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -61,10 +61,13 @@ class TreeExecutionServer
   rclcpp::Node::SharedPtr node();
 
   /// @brief Name of the tree being executed
-  const std::string& currentTreeName() const;
+  const std::string& treeName() const;
 
-  /// @brief Tree being executed, nullptr if it doesn't exist, yet.
-  BT::Tree* currentTree();
+  /// @brief The payload received in the last goal
+  const std::string& goalPayload() const;
+
+  /// @brief Tree being executed.
+  const BT::Tree& tree() const;
 
   /// @brief Pointer to the global blackboard
   BT::Blackboard::Ptr globalBlackboard();
@@ -110,9 +113,14 @@ class TreeExecutionServer
    *
    * @param status The status of the tree after the last tick
    * @param was_cancelled True if the action was cancelled by the Action Client
+   *
+   * @return if not std::nullopt, the string will be sent as [return_message] to the Action Client.
   */
-  virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled)
-  {}
+  virtual std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status,
+                                                              bool was_cancelled)
+  {
+    return std::nullopt;
+  }
 
   /**
    * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce().

behaviortree_ros2/src/tree_execution_server.cpp

@@ -32,17 +32,16 @@ struct TreeExecutionServer::Pimpl
 {
   rclcpp_action::Server<ExecuteTree>::SharedPtr action_server;
   std::thread action_thread;
-  // thread safe bool to keep track of cancel requests
-  std::atomic<bool> cancel_requested{ false };
 
   std::shared_ptr<bt_server::ParamListener> param_listener;
   bt_server::Params params;
 
   BT::BehaviorTreeFactory factory;
   std::shared_ptr<BT::Groot2Publisher> groot_publisher;
 
-  std::string current_tree_name;
-  std::shared_ptr<BT::Tree> tree;
+  std::string tree_name;
+  std::string payload;
+  BT::Tree tree;
   BT::Blackboard::Ptr global_blackboard;
   bool factory_initialized_ = false;
 
@@ -122,7 +121,6 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
 {
   RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s",
               goal->target_tree.c_str());
-  p_->cancel_requested = false;
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 
@@ -136,7 +134,6 @@ rclcpp_action::CancelResponse TreeExecutionServer::handle_cancel(
                          "processing one.");
     return rclcpp_action::CancelResponse::REJECT;
   }
-  p_->cancel_requested = true;
   return rclcpp_action::CancelResponse::ACCEPT;
 }
 
@@ -171,43 +168,50 @@ void TreeExecutionServer::execute(
     // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
     auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
 
-    p_->tree = std::make_shared<BT::Tree>();
-    *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard);
-    p_->current_tree_name = goal->target_tree;
+    p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);
+    p_->tree_name = goal->target_tree;
+    p_->payload = goal->payload;
 
     // call user defined function after the tree has been created
-    onTreeCreated(*p_->tree);
+    onTreeCreated(p_->tree);
     p_->groot_publisher.reset();
     p_->groot_publisher =
-        std::make_shared<BT::Groot2Publisher>(*(p_->tree), p_->params.groot2_port);
+        std::make_shared<BT::Groot2Publisher>(p_->tree, p_->params.groot2_port);
 
     // Loop until the tree is done or a cancel is requested
     const auto period =
         std::chrono::milliseconds(static_cast<int>(1000.0 / p_->params.tick_frequency));
     auto loop_deadline = std::chrono::steady_clock::now() + period;
 
     // operations to be done if the tree execution is aborted, either by
-    // cancel_requested_ or by onLoopAfterTick()
+    // cancel requested or by onLoopAfterTick()
     auto stop_action = [this, &action_result](BT::NodeStatus status,
                                               const std::string& message) {
+      p_->tree.haltTree();
       action_result->node_status = ConvertNodeStatus(status);
-      action_result->error_message = message;
-      RCLCPP_WARN(kLogger, action_result->error_message.c_str());
-      p_->tree->haltTree();
-      onTreeExecutionCompleted(status, true);
+      // override the message value if the user defined function returns it
+      if(auto msg = onTreeExecutionCompleted(status, true))
+      {
+        action_result->return_message = msg.value();
+      }
+      else
+      {
+        action_result->return_message = message;
+      }
+      RCLCPP_WARN(kLogger, action_result->return_message.c_str());
     };
 
     while(rclcpp::ok() && status == BT::NodeStatus::RUNNING)
     {
-      if(p_->cancel_requested)
+      if(goal_handle->is_canceling())
       {
         stop_action(status, "Action Server canceling and halting Behavior Tree");
         goal_handle->canceled(action_result);
         return;
       }
 
       // tick the tree once and publish the action feedback
-      status = p_->tree->tickExactlyOnce();
+      status = p_->tree.tickExactlyOnce();
 
       if(const auto res = onLoopAfterTick(status); res.has_value())
       {
@@ -219,56 +223,67 @@ void TreeExecutionServer::execute(
       if(const auto res = onLoopFeedback(); res.has_value())
       {
         auto feedback = std::make_shared<ExecuteTree::Feedback>();
-        feedback->msg = res.value();
+        feedback->message = res.value();
         goal_handle->publish_feedback(feedback);
       }
 
       const auto now = std::chrono::steady_clock::now();
       if(now < loop_deadline)
       {
-        p_->tree->sleep(loop_deadline - now);
+        p_->tree.sleep(loop_deadline - now);
       }
       loop_deadline += period;
     }
   }
   catch(const std::exception& ex)
   {
-    action_result->error_message = std::string("Behavior Tree exception:") + ex.what();
-    RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
+    action_result->return_message = std::string("Behavior Tree exception:") + ex.what();
+    RCLCPP_ERROR(kLogger, action_result->return_message.c_str());
     goal_handle->abort(action_result);
     return;
   }
 
-  // call user defined execution complete function
-  onTreeExecutionCompleted(status, false);
+  // Call user defined onTreeExecutionCompleted function.
+  // Override the message value if the user defined function returns it
+  if(auto msg = onTreeExecutionCompleted(status, false))
+  {
+    action_result->return_message = msg.value();
+  }
+  else
+  {
+    action_result->return_message =
+        std::string("Tree finished with status: ") + BT::toStr(status);
+  }
 
   // set the node_status result to the action
   action_result->node_status = ConvertNodeStatus(status);
 
   // return success or aborted for the action result
   if(status == BT::NodeStatus::SUCCESS)
   {
-    RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str());
+    RCLCPP_INFO(kLogger, action_result->return_message.c_str());
     goal_handle->succeed(action_result);
   }
   else
   {
-    action_result->error_message = std::string("Behavior Tree failed during execution "
-                                               "with status: ") +
-                                   BT::toStr(status);
-    RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
+    RCLCPP_ERROR(kLogger, action_result->return_message.c_str());
     goal_handle->abort(action_result);
   }
 }
 
-const std::string& TreeExecutionServer::currentTreeName() const
+const std::string& TreeExecutionServer::treeName() const
+{
+  return p_->tree_name;
+}
+
+const std::string& TreeExecutionServer::goalPayload() const
 {
-  return p_->current_tree_name;
+  return p_->payload;
 }
 
-BT::Tree* TreeExecutionServer::currentTree()
+const BT::Tree& TreeExecutionServer::tree() const
 {
-  return p_->tree ? p_->tree.get() : nullptr;
+  return p_->tree;
 }
 
 BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard()

btcpp_ros2_interfaces/action/ExecuteTree.action

@@ -1,9 +1,18 @@
-# Request
+#### Request ####
+
+# Name of the tree to execute
 string target_tree
+# Optional, implementation-dependent, payload.
+string payload
 ---
-# Result
-string error_message
+#### Result ####
+
+# Status of the tree
 NodeStatus node_status
+# result payload or error message
+string return_message
 ---
-# Feedback. This can be customized by the user
-string msg
+#### Feedback ####
+
+#This can be customized by the user
+string message

btcpp_ros2_samples/src/sample_bt_executor.cpp

@@ -44,11 +44,13 @@ class MyActionServer : public BT::TreeExecutionServer
     logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
   }
 
-  void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override
+  std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status,
+                                                      bool was_cancelled) override
   {
     // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree
     // at this point
     logger_cout_.reset();
+    return std::nullopt;
   }
 
 private: