improve blackboard exporting

Author: facontidavide

src/blackboard.cpp

@@ -1,4 +1,5 @@
 #include "behaviortree_cpp/blackboard.h"
+#include <unordered_set>
 #include "behaviortree_cpp/json_export.h"
 
 namespace BT
@@ -166,15 +167,44 @@ void Blackboard::createEntry(const std::string& key, const TypeInfo& info)
 
 void Blackboard::cloneInto(Blackboard& dst) const
 {
-  std::unique_lock lk(dst.mutex_);
-  dst.storage_.clear();
+  std::unique_lock lk1(mutex_);
+  std::unique_lock lk2(dst.mutex_);
 
-  for(const auto& [key, entry] : storage_)
+  // keys that are not updated must be removed.
+  std::unordered_set<std::string> keys_to_remove;
+  auto& dst_storage = dst.storage_;
+  for(const auto& [key, _] : dst_storage)
+  {
+    keys_to_remove.insert(key);
+  }
+
+  // update or create entries in dst_storage
+  for(const auto& [src_key, src_entry] : storage_)
+  {
+    keys_to_remove.erase(src_key);
+
+    auto it = dst_storage.find(src_key);
+    if(it != dst_storage.end())
+    {
+      // overwite
+      auto& dst_entry = it->second;
+      dst_entry->string_converter = src_entry->string_converter;
+      dst_entry->value = src_entry->value;
+      dst_entry->info = src_entry->info;
+    }
+    else
+    {
+      // create new
+      auto new_entry = std::make_shared<Entry>(src_entry->info);
+      new_entry->value = src_entry->value;
+      new_entry->string_converter = src_entry->string_converter;
+      dst_storage.insert({ src_key, new_entry });
+    }
+  }
+
+  for(const auto& key : keys_to_remove)
   {
-    auto new_entry = std::make_shared<Entry>(entry->info);
-    new_entry->value = entry->value;
-    new_entry->string_converter = entry->string_converter;
-    dst.storage_.insert({ key, new_entry });
+    dst_storage.erase(key);
   }
 }
 

src/bt_factory.cpp

@@ -697,25 +697,32 @@ std::vector<Blackboard::Ptr> BlackboardBackup(const Tree& tree)
 
 nlohmann::json ExportTreeToJSON(const Tree& tree)
 {
-  std::vector<nlohmann::json> bbs;
+  nlohmann::json out;
   for(const auto& subtree : tree.subtrees)
   {
-    bbs.push_back(ExportBlackboardToJSON(*subtree->blackboard));
+    nlohmann::json json_sub;
+    auto sub_name = subtree->instance_name;
+    if(sub_name.empty())
+    {
+      sub_name = subtree->tree_ID;
+    }
+    out[sub_name] = ExportBlackboardToJSON(*subtree->blackboard);
   }
-  return bbs;
+  return out;
 }
 
 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++)
+
+  size_t index = 0;
+  for(auto& [key, array] : json.items())
   {
-    ImportBlackboardFromJSON(json.at(i), *tree.subtrees.at(i)->blackboard);
+    auto& subtree = tree.subtrees.at(index++);
+    ImportBlackboardFromJSON(array, *subtree->blackboard);
   }
 }