From 9c644e18272ad2e26b75e7dc3707a57d3326e30a Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 24 Apr 2024 18:37:05 +0200
Subject: [PATCH] Update TestNode and the corresponding tutorial

---
 examples/t11_replace_rules.cpp               | 176 +++++++++++--------
 include/behaviortree_cpp/actions/test_node.h |  20 ++-
 src/actions/test_node.cpp                    |  42 +++--
 src/bt_factory.cpp                           |  26 +--
 4 files changed, 155 insertions(+), 109 deletions(-)

diff --git a/examples/t11_replace_rules.cpp b/examples/t11_replace_rules.cpp
index 3bcfebd15..f8879790c 100644
--- a/examples/t11_replace_rules.cpp
+++ b/examples/t11_replace_rules.cpp
@@ -9,14 +9,17 @@ static const char* xml_text = R"(
   <BehaviorTree ID="MainTree">
     <Sequence>
       <SaySomething name="talk" message="hello world"/>
-        <Fallback>
-          <AlwaysFailure name="failing_action"/>
-          <SubTree ID="MySub" name="mysub"/>
-        </Fallback>
-        <SaySomething message="before last_action"/>
-        <Script code="msg:='after last_action'"/>
-        <AlwaysSuccess name="last_action"/>
-        <SaySomething message="{msg}"/>
+
+      <SubTree ID="MySub" name="mysub"/>
+
+      <Script name="set_message" code="msg:= 'the original message' "/>
+      <SaySomething message="{msg}"/>
+
+      <Sequence name="counting">
+        <SaySomething message="1"/>
+        <SaySomething message="2"/>
+        <SaySomething message="3"/>
+      </Sequence>
     </Sequence>
   </BehaviorTree>
 
@@ -30,95 +33,126 @@ static const char* xml_text = R"(
 </root>
  )";
 
-static const char* json_text = R"(
-{
-  "TestNodeConfigs": {
-    "MyTest": {
-      "async_delay": 2000,
-      "return_status": "SUCCESS",
-      "post_script": "msg ='message SUBSTITUED'"
-    }
-  },
-
-  "SubstitutionRules": {
-    "mysub/action_*": "TestAction",
-    "talk": "TestSaySomething",
-    "last_action": "MyTest"
-  }
-}
- )";
-
 // clang-format on
 
+/**
+ * @brief In this example we will see how we can substitute some nodes
+ * in the Tree above with
+ * @param argc
+ * @param argv
+ * @return
+ */
+
 int main(int argc, char** argv)
 {
   using namespace DummyNodes;
   BT::BehaviorTreeFactory factory;
-
   factory.registerNodeType<SaySomething>("SaySomething");
+  factory.registerBehaviorTreeFromText(xml_text);
 
-  // We use lambdas and registerSimpleAction, to create
-  // a "dummy" node, that we want to create instead of a given one.
+  // let's check what the "original" tree should return
+  {
+    auto tree = factory.createTree("MainTree");
+
+    std::cout << "----- Nodes fullPath() -------\n";
+    // as a reminder, let's print the full names of all the nodes
+    tree.applyVisitor(
+        [](BT::TreeNode* node) { std::cout << node->fullPath() << std::endl; });
+
+    std::cout << "\n------ Output (original) ------\n";
+    tree.tickWhileRunning();
+  }
+
+  // We have three mechanisms to create Nodes to be used as "mocks".
+  // We will see later how to use them.
+
+  //---------------------------------------------------------------
+  // Mock type 1: register a specific "dummy" Node into the factory
+  // You can use any registration method, but to keep this short,
+  // we use registerSimpleAction()
 
-  // Simple node that just prints its name and return SUCCESS
   factory.registerSimpleAction("DummyAction", [](BT::TreeNode& self) {
-    std::cout << "DummyAction substituting: " << self.name() << std::endl;
+    std::cout << "DummyAction substituting node with fullPath(): " << self.fullPath()
+              << std::endl;
     return BT::NodeStatus::SUCCESS;
   });
 
-  // Action that is meant to substitute SaySomething.
-  // It will try to use the input port "message"
-  factory.registerSimpleAction("TestSaySomething", [](BT::TreeNode& self) {
+  factory.registerSimpleAction("DummySaySomething", [](BT::TreeNode& self) {
     auto msg = self.getInput<std::string>("message");
-    if(!msg)
-    {
-      throw BT::RuntimeError("missing required input [message]: ", msg.error());
-    }
-    std::cout << "TestSaySomething: " << msg.value() << std::endl;
+    std::cout << "DummySaySomething: " << msg.value() << std::endl;
     return BT::NodeStatus::SUCCESS;
   });
 
-  //----------------------------
-  // pass "no_sub" as first argument to avoid adding rules
-  bool skip_substitution = (argc == 2) && std::string(argv[1]) == "no_sub";
+  //---------------------------------------------------------------
+  // Mock type 2: Use our configurable BT::TestNode
+
+  // This is the configuration passed to the TestNode
+  BT::TestNodeConfig test_config;
+  // we want this to return always SUCCESS
+  test_config.return_status = BT::NodeStatus::SUCCESS;
+  // Convert the node in asynchronous and wait 2000 ms
+  test_config.async_delay = std::chrono::milliseconds(2000);
+  // Execute this postcondition, once completed
+  test_config.post_script = "msg := 'message SUBSTITUTED' ";
 
-  if(!skip_substitution)
+  // this will be synchronous (async_delay is 0)
+  BT::TestNodeConfig counting_config;
+  test_config.return_status = BT::NodeStatus::SUCCESS;
+
+  //---------------------------------------------------------------
+  // Next, we want to substitute one or more of out Nodes with this mocks
+  // The simplest way is to use a JSON file, otherwise we can do it manually.
+  bool const USE_JSON = true;
+
+  if(!USE_JSON)  // manually add substitution rules
   {
-    // we can use a JSON file to configure the substitution rules
-    // or do it manually
-    bool const USE_JSON = true;
+    // Substitute nodes which match the wildcard pattern "mysub/action_*"
+    // with DummyAction
+    factory.addSubstitutionRule("mysub/action_*", "DummyAction");
 
-    if(USE_JSON)
-    {
-      factory.loadSubstitutionRuleFromJSON(json_text);
-    }
-    else
-    {
-      // Substitute nodes which match this wildcard pattern with TestAction
-      factory.addSubstitutionRule("mysub/action_*", "TestAction");
-
-      // Substitute the node with name [talk] with TestSaySomething
-      factory.addSubstitutionRule("talk", "TestSaySomething");
-
-      // This configuration will be passed to a TestNode
-      BT::TestNodeConfig test_config;
-      // Convert the node in asynchronous and wait 2000 ms
-      test_config.async_delay = std::chrono::milliseconds(2000);
-      // Execute this postcondition, once completed
-      test_config.post_script = "msg ='message SUBSTITUED'";
-
-      // Substitute the node with name [last_action] with a TestNode,
-      // configured using test_config
-      factory.addSubstitutionRule("last_action", test_config);
-    }
-  }
+    // Substitute the node with name "talk" with DummySaySomething
+    factory.addSubstitutionRule("talk", "DummySaySomething");
 
-  factory.registerBehaviorTreeFromText(xml_text);
+    // Substitute the node with name "set_message" with
+    // the a BT::TestNode with the give configuration
+    factory.addSubstitutionRule("set_message", test_config);
 
+    // we can also substitute entire branches, for instance the Sequence "counting"
+    factory.addSubstitutionRule("counting", counting_config);
+  }
+  else  // use a JSON file to apply substitution rules programmatically
+  {
+    // this JSON is equivalent to the code we wrote above
+    const char* json_text = R"(
+    {
+      "TestNodeConfigs": {
+        "NewMessage": {
+          "async_delay": 2000,
+          "return_status": "SUCCESS",
+          "post_script": "msg ='message SUBSTITUTED'"
+        },
+        "NoCounting": {
+          "return_status": "SUCCESS"
+        }
+      },
+
+      "SubstitutionRules": {
+        "mysub/action_*": "DummyAction",
+        "talk": "DummySaySomething",
+        "set_message": "NewMessage",
+        "counting": "NoCounting"
+      }
+    })";
+
+    factory.loadSubstitutionRuleFromJSON(json_text);
+  }
+  //---------------------------------------------------------------
+  // IMPORTANT: all substiutions must be done BEFORE creating the tree
   // During the construction phase of the tree, the substitution
   // rules will be used to instantiate the test nodes, instead of the
   // original ones.
   auto tree = factory.createTree("MainTree");
+  std::cout << "\n------ Output (substituted) ------\n";
   tree.tickWhileRunning();
 
   return 0;
diff --git a/include/behaviortree_cpp/actions/test_node.h b/include/behaviortree_cpp/actions/test_node.h
index 43c48e6ba..42db7d855 100644
--- a/include/behaviortree_cpp/actions/test_node.h
+++ b/include/behaviortree_cpp/actions/test_node.h
@@ -22,20 +22,24 @@ namespace BT
 
 struct TestNodeConfig
 {
-  /// status to return when the action is completed
+  /// status to return when the action is completed.
   NodeStatus return_status = NodeStatus::SUCCESS;
 
+  /// script to execute when complete_func() returns SUCCESS
+  std::string success_script;
+
+  /// script to execute when complete_func() returns FAILURE
+  std::string failure_script;
+
   /// script to execute when actions is completed
   std::string post_script;
 
   /// if async_delay > 0, this action become asynchronous and wait this amount of time
   std::chrono::milliseconds async_delay = std::chrono::milliseconds(0);
 
-  /// C++ callback to execute at the beginning
-  std::function<void()> pre_func;
-
-  /// C++ callback to execute at the end
-  std::function<void()> post_func;
+  /// Function invoked when the action is completed. By default just return [return_status]
+  /// Override it to intorduce more comple cases
+  std::function<NodeStatus(void)> complete_func = [this]() { return return_status; };
 };
 
 /**
@@ -84,7 +88,9 @@ class TestNode : public BT::StatefulActionNode
   NodeStatus onCompleted();
 
   TestNodeConfig _test_config;
-  ScriptFunction _executor;
+  ScriptFunction _success_executor;
+  ScriptFunction _failure_executor;
+  ScriptFunction _post_executor;
   TimerQueue<> _timer;
   std::atomic_bool _completed = false;
 };
diff --git a/src/actions/test_node.cpp b/src/actions/test_node.cpp
index 0df48d4f6..361994da6 100644
--- a/src/actions/test_node.cpp
+++ b/src/actions/test_node.cpp
@@ -8,24 +8,24 @@ void BT::TestNode::setConfig(const TestNodeConfig& config)
   }
   _test_config = config;
 
-  if(!_test_config.post_script.empty())
-  {
-    auto executor = ParseScript(_test_config.post_script);
-    if(!executor)
+  auto prepareScript = [](const std::string& script, auto& executor) {
+    if(!script.empty())
     {
-      throw RuntimeError(executor.error());
+      auto result = ParseScript(script);
+      if(!result)
+      {
+        throw RuntimeError(result.error());
+      }
+      executor = result.value();
     }
-    _executor = executor.value();
-  }
+  };
+  prepareScript(config.success_script, _success_executor);
+  prepareScript(config.failure_script, _failure_executor);
+  prepareScript(config.post_script, _post_executor);
 }
 
 BT::NodeStatus BT::TestNode::onStart()
 {
-  if(_test_config.pre_func)
-  {
-    _test_config.pre_func();
-  }
-
   if(_test_config.async_delay <= std::chrono::milliseconds(0))
   {
     return onCompleted();
@@ -63,14 +63,20 @@ void BT::TestNode::onHalted()
 
 BT::NodeStatus BT::TestNode::onCompleted()
 {
-  if(_executor)
+  Ast::Environment env = { config().blackboard, config().enums };
+
+  auto status = _test_config.complete_func();
+  if(status == NodeStatus::SUCCESS && _success_executor)
+  {
+    _success_executor(env);
+  }
+  else if(status == NodeStatus::FAILURE && _failure_executor)
   {
-    Ast::Environment env = { config().blackboard, config().enums };
-    _executor(env);
+    _failure_executor(env);
   }
-  if(_test_config.post_func)
+  if(_post_executor)
   {
-    _test_config.post_func();
+    _post_executor(env);
   }
-  return _test_config.return_status;
+  return status;
 }
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
index c130d4ac7..1a91b2ac1 100644
--- a/src/bt_factory.cpp
+++ b/src/bt_factory.cpp
@@ -326,7 +326,7 @@ std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
         }
         else
         {
-          throw RuntimeError("Substituted Node ID not found");
+          throw RuntimeError("Substituted Node ID [", *substituted_ID, "] not found");
         }
         substituted = true;
         break;
@@ -490,8 +490,8 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
   {
     auto& config = configs[name];
 
-    auto status = test_config.at("return_status").get<std::string>();
-    config.return_status = convertFromString<NodeStatus>(status);
+    auto return_status = test_config.at("return_status").get<std::string>();
+    config.return_status = convertFromString<NodeStatus>(return_status);
     if(test_config.contains("async_delay"))
     {
       config.async_delay =
@@ -501,6 +501,14 @@ void BehaviorTreeFactory::loadSubstitutionRuleFromJSON(const std::string& json_t
     {
       config.post_script = test_config["post_script"].get<std::string>();
     }
+    if(test_config.contains("success_script"))
+    {
+      config.success_script = test_config["success_script"].get<std::string>();
+    }
+    if(test_config.contains("failure_script"))
+    {
+      config.failure_script = test_config["failure_script"].get<std::string>();
+    }
   }
 
   auto substitutions = json.at("SubstitutionRules");
@@ -616,20 +624,12 @@ Blackboard::Ptr Tree::rootBlackboard()
 
 void Tree::applyVisitor(const std::function<void(const TreeNode*)>& visitor)
 {
-  for(auto const& subtree : subtrees)
-  {
-    BT::applyRecursiveVisitor(static_cast<const TreeNode*>(subtree->nodes.front().get()),
-                              visitor);
-  }
+  BT::applyRecursiveVisitor(static_cast<const TreeNode*>(rootNode()), visitor);
 }
 
 void Tree::applyVisitor(const std::function<void(TreeNode*)>& visitor)
 {
-  for(auto const& subtree : subtrees)
-  {
-    BT::applyRecursiveVisitor(static_cast<TreeNode*>(subtree->nodes.front().get()),
-                              visitor);
-  }
+  BT::applyRecursiveVisitor(static_cast<TreeNode*>(rootNode()), visitor);
 }
 
 uint16_t Tree::getUID()