add payloads to Action

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 The payload received in the last goal
+ const std::string& goalPayload() const;
 
  /// @brief Tree being executed, nullptr if it doesn't exist, yet.
- BT::Tree* currentTree();
+ BT::Tree* tree();
 
  /// @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

@@ -41,7 +41,8 @@ struct TreeExecutionServer::Pimpl
  BT::BehaviorTreeFactory factory;
  std::shared_ptr<BT::Groot2Publisher> groot_publisher;
 
- std::string current_tree_name;
+ std::string tree_name;
+ std::string payload;
  std::shared_ptr<BT::Tree> tree;
  BT::Blackboard::Ptr global_blackboard;
  bool factory_initialized_ = false;
@@ -173,7 +174,8 @@ void TreeExecutionServer::execute(
 
  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_name = goal->target_tree;
+ p_->payload = goal->payload;
 
  // call user defined function after the tree has been created
  onTreeCreated(*p_->tree);
@@ -191,10 +193,14 @@ void TreeExecutionServer::execute(
  auto stop_action = [this, &action_result](BT::NodeStatus status,
  const std::string& message) {
  action_result->node_status = ConvertNodeStatus(status);
- action_result->error_message = message;
- RCLCPP_WARN(kLogger, action_result->error_message.c_str());
+ action_result->return_message = message;
+ RCLCPP_WARN(kLogger, action_result->return_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();
+ }
  };
 
  while(rclcpp::ok() && status == BT::NodeStatus::RUNNING)
@@ -219,7 +225,7 @@ 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);
  }
 
@@ -233,40 +239,38 @@ void TreeExecutionServer::execute(
  }
  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);
-
  // 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());
- goal_handle->succeed(action_result);
- }
- else
+ // Call user defined onTreeExecutionCompleted function.
+ // Override the message value if the user defined function returns it
+ if(auto msg = onTreeExecutionCompleted(status, false))
  {
- 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());
- goal_handle->abort(action_result);
+ action_result->return_message = msg.value();
  }
+
+ // return success or aborted for the action result
+ RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str());
+ goal_handle->succeed(action_result);
+}
+
+const std::string& TreeExecutionServer::treeName() const
+{
+ return p_->tree_name;
 }
 
-const std::string& TreeExecutionServer::currentTreeName() const
+const std::string& TreeExecutionServer::goalPayload() const
 {
- return p_->current_tree_name;
+ return p_->payload;
 }
 
-BT::Tree* TreeExecutionServer::currentTree()
+BT::Tree* TreeExecutionServer::tree()
 {
  return p_->tree ? p_->tree.get() : nullptr;
 }

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: