add blackboard backup/restore

Author: facontidavide

examples/CMakeLists.txt

@@ -38,6 +38,7 @@ CompileExample("ex02_runtime_ports")
 CompileExample("ex04_waypoints")
 CompileExample("ex05_subtree_model")
 CompileExample("ex06_access_by_ptr")
+CompileExample("ex07_blackboard_backup")
 
 CompileExample("t13_plugin_executor")
 

examples/ex07_blackboard_backup.cpp

@@ -0,0 +1,67 @@
+#include "behaviortree_cpp/bt_factory.h"
+#include "dummy_nodes.h"
+
+using namespace BT;
+
+// clang-format off
+static const char* xml_tree = R"(
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <Script code="val_A:= 'john' "/>
+      <Script code="val_B:= 42 "/>
+      <SaySomething message="{val_A}" />
+      <SaySomething message="hello world" />
+      <SubTree ID="Sub" val="{val_A}" _autoremap="true" />
+      <SaySomething message="{reply}" />
+    </Sequence>
+  </BehaviorTree>
+  <BehaviorTree ID="Sub">
+    <Sequence>
+      <SaySomething message="{val}" />
+      <SaySomething message="{val_B}" />
+      <Script code="reply:= 'done' "/>
+    </Sequence>
+  </BehaviorTree>
+</root>
+ )";
+
+// clang-format on
+
+
+int main()
+{
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerBehaviorTreeFromText(xml_tree);
+
+  auto tree = factory.createTree("MainTree");
+
+  // We want to create a memory of the blackboard in memory.
+  // This is conveninet when we want to reset its state to the
+  // original one.
+  // It is certainly more efficient than destroying and creating the tree again,
+  // in many casess.
+
+  const auto backup_before_tick = BlackboardBackup(tree);
+  tree.tickWhileRunning();
+
+  // Restore the original status of the blackboard
+  BlackboardRestore(backup_before_tick, tree);
+  tree.tickWhileRunning();
+
+  // Alternatively, we may want to save he values of the element in the blackboard
+  // to file, to restore them again. We use JSON serialization for that.
+  nlohmann::json json_after_tick = ExportTreeToJSON(tree);
+
+  // The JSOn object can be saved to file. See https://github.com/nlohmann/json
+  // for details. For the time being, we just print it in the console
+
+  std::cout << "--- blackboard serialized as JSON: ----\n"
+            << json_after_tick.dump(2) << std::endl;
+
+  // We can restore the values of the blackboards using the JSON
+  ImportTreeFromJSON(json_after_tick, tree);
+
+  return 0;
+}

include/behaviortree_cpp/bt_factory.h

@@ -507,6 +507,44 @@ class BehaviorTreeFactory
 
 };
 
+/**
+ * @brief BlackboardClone make a copy of the content of the
+ * blackboard
+ * @param src   source
+ * @param dst   destination
+ */
+void BlackboardClone(const Blackboard& src, Blackboard& dst);
+
+/**
+ * @brief BlackboardBackup uses BlackboardClone to backup
+ * all the blackboards of the tree
+ *
+ * @param tree source
+ * @return destination (the backup)
+ */
+std::vector<Blackboard::Ptr> BlackboardBackup(const BT::Tree& tree);
+
+/**
+ * @brief BlackboardRestore uses BlackboardClone to restore
+ * all the blackboards of the tree
+ *
+ * @param backup a vectror of blackboards
+ * @param tree the destination
+ */
+void BlackboardRestore(const std::vector<Blackboard::Ptr>& backup, BT::Tree& tree);
+
+/**
+ * @brief ExportTreeToJSON it calls ExportBlackboardToJSON
+ * for all the blackboards in the tree
+ */
+nlohmann::json ExportTreeToJSON(const BT::Tree &tree);
+
+/**
+ * @brief ImportTreeFromJSON it calls ImportBlackboardFromJSON
+ * for all the blackboards in the tree
+ */
+void ImportTreeFromJSON(const nlohmann::json &json, BT::Tree &tree);
+
 }   // namespace BT
 
 #endif   // BT_FACTORY_H

src/bt_factory.cpp

@@ -659,4 +659,63 @@ NodeStatus Tree::tickRoot(TickOption opt, std::chrono::milliseconds sleep_time)
   return status;
 }
 
+void BlackboardClone(const Blackboard &src, Blackboard &dst)
+{
+  dst.clear();
+  for(auto const key_name: src.getKeys())
+  {
+    const auto key = std::string(key_name);
+    const auto entry = src.getEntry(key);
+    dst.createEntry(key, entry->info);
+    auto new_entry =  dst.getEntry(key);
+    new_entry->value = entry->value;
+    new_entry->string_converter = entry->string_converter;
+  }
+}
+
+void BlackboardRestore(const std::vector<Blackboard::Ptr> &backup, Tree &tree)
+{
+  assert(backup.size() == tree.subtrees.size());
+  for(size_t i=0; i<tree.subtrees.size(); i++)
+  {
+    BlackboardClone(*(backup[i]), *(tree.subtrees[i]->blackboard));
+  }
+}
+
+std::vector<Blackboard::Ptr> BlackboardBackup(const Tree &tree)
+{
+  std::vector<Blackboard::Ptr> bb;
+  bb.reserve(tree.subtrees.size());
+  for(const auto& sub: tree.subtrees)
+  {
+    bb.push_back( BT::Blackboard::create() );
+    BlackboardClone(*sub->blackboard, *bb.back());
+  }
+  return bb;
+}
+
+nlohmann::json ExportTreeToJSON(const Tree &tree)
+{
+  std::vector<nlohmann::json> bbs;
+  for(const auto& subtree: tree.subtrees)
+  {
+    bbs.push_back(ExportBlackboardToJSON(*subtree->blackboard));
+  }
+  return bbs;
+}
+
+void ImportTreeFromJSON(const nlohmann::json &json, Tree &tree)
+{
+  if(json.size() != tree.subtrees.size())
+  {
+    std::cerr << "Number of blackboards don't match:"
+              << json.size() << "/" << tree.subtrees.size() << "\n";
+    throw std::runtime_error("Number of blackboards don't match:");
+  }
+  for(size_t i=0; i<tree.subtrees.size(); i++)
+  {
+    ImportBlackboardFromJSON(json.at(i), *tree.subtrees.at(i)->blackboard);
+  }
+}
+
 }   // namespace BT

tests/gtest_json.cpp

@@ -121,12 +121,12 @@ TEST_F(JsonTest, ConvertFromString)
 
 TEST_F(JsonTest, BlackboardInOut)
 {
-  auto bb = BT::Blackboard::create();
-  bb->set("int", 42);
-  bb->set("real", 3.14);
-  bb->set("vect", TestTypes::Vector3D{1.1, 2.2, 3.3});
+  auto bb_in = BT::Blackboard::create();
+  bb_in->set("int", 42);
+  bb_in->set("real", 3.14);
+  bb_in->set("vect", TestTypes::Vector3D{1.1, 2.2, 3.3});
 
-  auto json = ExportBlackboardToJSON(*bb);
+  auto json = ExportBlackboardToJSON(*bb_in);
   std::cout << json.dump(2) << std::endl;
 
   auto bb_out = BT::Blackboard::create();