From 78c13b7d761cc154c0ab624e13e6c0353e8ccbbf Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 28 Feb 2024 21:59:06 +0100
Subject: [PATCH] new fromJSON API

---
 include/behaviortree_cpp/blackboard.h  |  4 +--
 include/behaviortree_cpp/json_export.h | 36 ++++++++++++++----------
 src/basic_types.cpp                    |  2 +-
 src/blackboard.cpp                     | 16 +++++++++++
 src/json_export.cpp                    | 22 +++++++++------
 tests/gtest_json.cpp                   | 39 +++++++++++++++++++++-----
 6 files changed, 86 insertions(+), 33 deletions(-)

diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h
index 5c45d2778..f58e14bb2 100644
--- a/include/behaviortree_cpp/blackboard.h
+++ b/include/behaviortree_cpp/blackboard.h
@@ -125,11 +125,11 @@ class Blackboard
 nlohmann::json ExportBlackboardToJSON(const Blackboard &blackboard);
 
 /**
- * @brief ImportBlackboardToJSON will append elements to the blackboard,
+ * @brief ImportBlackboardFromJSON will append elements to the blackboard,
  * using the values parsed from the JSON file created using ExportBlackboardToJSON.
  * Complex types must be registered with JsonExporter::get()
  */
-void ImportBlackboardToJSON(const nlohmann::json& json, Blackboard& blackboard);
+void ImportBlackboardFromJSON(const nlohmann::json& json, Blackboard& blackboard);
 
 
 //------------------------------------------------------
diff --git a/include/behaviortree_cpp/json_export.h b/include/behaviortree_cpp/json_export.h
index 317db3ec2..db4e20c47 100644
--- a/include/behaviortree_cpp/json_export.h
+++ b/include/behaviortree_cpp/json_export.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/utils/safe_any.hpp"
 #include "behaviortree_cpp/contrib/expected.hpp"
 
@@ -56,21 +57,26 @@ class JsonExporter {
    * @brief toJson adds the content of "any" to the JSON "destination".
    *
    * It will return false if the conversion toJson is not possible
-   * ( you might need to register the converter with addConverter() ).
+   * If it is a custom type, you might register it first with addConverter().
    */
   bool toJson(const BT::Any& any, nlohmann::json& destination) const;
 
-  using ExpectedAny = nonstd::expected_lite::expected<BT::Any, std::string>;
+  /// This information is needed to create a BT::Blackboard::entry
+  using Entry = std::pair<BT::Any, BT::TypeInfo>;
 
-  ExpectedAny fromJson(const nlohmann::json& source) const;
-
-  ExpectedAny fromJson(const nlohmann::json& source, std::type_index type) const;
+  using ExpectedEntry = nonstd::expected_lite::expected<Entry, std::string>;
 
+  /**
+   * @brief fromJson will return an Entry (value wrappedn in Any + TypeInfo)
+   * from a json source.
+   * If it is a custom type, you might register it first with addConverter().
+   * @param source
+   * @return
+   */
+  ExpectedEntry fromJson(const nlohmann::json& source) const;
 
-  template <typename T>
-  void toJson(const T& val, nlohmann::json& dst) const {
-    dst = val;
-  }
+  /// Same as the other, but providing the specific type
+  ExpectedEntry fromJson(const nlohmann::json& source, std::type_index type) const;
 
   /// Register new JSON converters with addConverter<Foo>().
   /// You should have used first the macro BT_JSON_CONVERTER
@@ -79,11 +85,11 @@ class JsonExporter {
 private:
 
   using ToJonConverter = std::function<void(const BT::Any&, nlohmann::json&)>;
-  using FromJonConverter = std::function<BT::Any(const nlohmann::json&)>;
+  using FromJonConverter = std::function<Entry(const nlohmann::json&)>;
 
   std::unordered_map<std::type_index, ToJonConverter> to_json_converters_;
   std::unordered_map<std::type_index, FromJonConverter> from_json_converters_;
-  std::unordered_map<std::string, std::type_index> type_names_;
+  std::unordered_map<std::string, BT::TypeInfo> type_names_;
 };
 
 
@@ -99,12 +105,12 @@ void JsonExporter::addConverter()
   };
   to_json_converters_.insert( {typeid(T), to_converter} );
 
-  FromJonConverter from_converter = [](const nlohmann::json& dst) -> BT::Any
+  FromJonConverter from_converter = [](const nlohmann::json& dst) -> Entry
   {
     T value;
     using namespace nlohmann;
     from_json(dst, value);
-    return BT::Any(value);
+    return {BT::Any(value), BT::TypeInfo::Create<T>()};
   };
 
   // we need to get the name of the type
@@ -112,9 +118,9 @@ void JsonExporter::addConverter()
   // we insert both the name obtained from JSON and demangle
   if(js.contains("__type"))
   {
-    type_names_.insert( {std::string(js["__type"]), typeid(T)} );
+    type_names_.insert( {std::string(js["__type"]), BT::TypeInfo::Create<T>()} );
   }
-  type_names_.insert( {BT::demangle(typeid(T)), typeid(T)} );
+  type_names_.insert( {BT::demangle(typeid(T)), BT::TypeInfo::Create<T>()} );
 
   from_json_converters_.insert( {typeid(T), from_converter} );
 }
diff --git a/src/basic_types.cpp b/src/basic_types.cpp
index 5c63a2eed..e7662467e 100644
--- a/src/basic_types.cpp
+++ b/src/basic_types.cpp
@@ -410,7 +410,7 @@ Any convertFromJSON(StringView json_text, std::type_index type)
   {
     throw std::runtime_error(res.error());
   }
-  return *res;
+  return res->first;
 }
 
 }   // namespace BT
diff --git a/src/blackboard.cpp b/src/blackboard.cpp
index 8467be200..636d4ddd2 100644
--- a/src/blackboard.cpp
+++ b/src/blackboard.cpp
@@ -242,4 +242,20 @@ nlohmann::json ExportBlackboardToJSON(const Blackboard &blackboard)
   return dest;
 }
 
+void ImportBlackboardFromJSON(const nlohmann::json &json, Blackboard &blackboard)
+{
+  for (auto it = json.begin(); it != json.end(); ++it)
+  {
+    if(auto res = JsonExporter::get().fromJson(it.value()))
+    {
+      auto entry = blackboard.getEntry(it.key());
+      if(!entry) {
+        blackboard.createEntry(it.key(), res->second);
+        entry = blackboard.getEntry(it.key());
+      }
+      entry->value = res->first;
+    }
+  }
+}
+
 }   // namespace BT
diff --git a/src/json_export.cpp b/src/json_export.cpp
index 73f47fdda..2ffb3d86d 100644
--- a/src/json_export.cpp
+++ b/src/json_export.cpp
@@ -38,7 +38,7 @@ bool JsonExporter::toJson(const Any &any, nlohmann::json &dst) const
   return true;
 }
 
-JsonExporter::ExpectedAny JsonExporter::fromJson(const nlohmann::json &source) const
+JsonExporter::ExpectedEntry JsonExporter::fromJson(const nlohmann::json &source) const
 {
   if(source.is_null())
   {
@@ -46,23 +46,28 @@ JsonExporter::ExpectedAny JsonExporter::fromJson(const nlohmann::json &source) c
   }
   if( source.is_string())
   {
-    return BT::Any(source.get<std::string>());
+    return Entry{BT::Any(source.get<std::string>()),
+                 BT::TypeInfo::Create<std::string>()};
   }
   if( source.is_number_unsigned())
   {
-    return BT::Any(source.get<uint64_t>());
+    return Entry{BT::Any(source.get<uint64_t>()),
+                 BT::TypeInfo::Create<uint64_t>()};
   }
   if( source.is_number_integer())
   {
-    return BT::Any(source.get<int64_t>());
+    return Entry{BT::Any(source.get<int64_t>()),
+                 BT::TypeInfo::Create<int64_t>()};
   }
   if( source.is_number_float())
   {
-    return BT::Any(source.get<double>());
+    return Entry{BT::Any(source.get<double>()),
+                 BT::TypeInfo::Create<double>()};
   }
   if( source.is_boolean())
   {
-    return BT::Any(source.get<bool>());
+    return Entry{BT::Any(source.get<bool>()),
+                 BT::TypeInfo::Create<bool>()};
   }
 
   if(!source.contains("__type"))
@@ -74,7 +79,7 @@ JsonExporter::ExpectedAny JsonExporter::fromJson(const nlohmann::json &source) c
   {
     return nonstd::make_unexpected("Type not found in registered list");
   }
-  auto func_it = from_json_converters_.find(type_it->second);
+  auto func_it = from_json_converters_.find(type_it->second.type());
   if(func_it == from_json_converters_.end())
   {
     return nonstd::make_unexpected("Type not found in registered list");
@@ -82,7 +87,8 @@ JsonExporter::ExpectedAny JsonExporter::fromJson(const nlohmann::json &source) c
   return func_it->second(source);
 }
 
-JsonExporter::ExpectedAny JsonExporter::fromJson(const nlohmann::json &source, std::type_index type) const
+JsonExporter::ExpectedEntry
+JsonExporter::fromJson(const nlohmann::json &source, std::type_index type) const
 {
   auto func_it = from_json_converters_.find(type);
   if(func_it == from_json_converters_.end())
diff --git a/tests/gtest_json.cpp b/tests/gtest_json.cpp
index b7894db64..8c04b6ef7 100644
--- a/tests/gtest_json.cpp
+++ b/tests/gtest_json.cpp
@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+#include "behaviortree_cpp/blackboard.h"
 #include "behaviortree_cpp/json_export.h"
 #include "behaviortree_cpp/basic_types.h"
 
@@ -90,7 +91,7 @@ TEST_F(JsonTest, TwoWaysConversion)
   ASSERT_EQ(json["pose"]["rot"]["z"], 7);
 
   // check the two-ways transform, i.e. "from_json"
-  auto pose2 = exporter.fromJson(json["pose"])->cast<TestTypes::Pose3D>();
+  auto pose2 = exporter.fromJson(json["pose"])->first.cast<TestTypes::Pose3D>();
 
   ASSERT_EQ(pose.pos.x, pose2.pos.x);
   ASSERT_EQ(pose.pos.y, pose2.pos.y);
@@ -101,19 +102,43 @@ TEST_F(JsonTest, TwoWaysConversion)
   ASSERT_EQ(pose.rot.y, pose2.rot.y);
   ASSERT_EQ(pose.rot.z, pose2.rot.z);
 
-  auto num = exporter.fromJson(json["int"])->cast<int>();
+  auto num = exporter.fromJson(json["int"])->first.cast<int>();
   ASSERT_EQ(num, 69);
-  auto real = exporter.fromJson(json["real"])->cast<double>();
+  auto real = exporter.fromJson(json["real"])->first.cast<double>();
   ASSERT_EQ(real, 3.14);
+}
 
+TEST_F(JsonTest, ConvertFromString)
+{
+  TestTypes::Vector3D vect;
+  auto const test_json = R"(json:{"x":2.1, "y":4.2, "z":6.3})";
+  ASSERT_NO_THROW(vect = BT::convertFromString<TestTypes::Vector3D>(test_json));
 
+  ASSERT_EQ(vect.x, 2.1);
+  ASSERT_EQ(vect.y, 4.2);
+  ASSERT_EQ(vect.z, 6.3);
 }
 
-TEST_F(JsonTest, ConvertFromString)
+TEST_F(JsonTest, BlackboardInOut)
 {
-  TestTypes::Vector3D pose3;
-  auto const test_json = R"(json:{"x":2, "y":4, "z":6})";
-  ASSERT_NO_THROW(pose3 = BT::convertFromString<TestTypes::Vector3D>(test_json));
+  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 json = ExportBlackboardToJSON(*bb);
+  std::cout << json.dump(2) << std::endl;
+
+  auto bb_out = BT::Blackboard::create();
+  ImportBlackboardFromJSON(json, *bb_out);
+
+  ASSERT_EQ(bb_out->get<int>("int"), 42);
+  ASSERT_EQ(bb_out->get<double>("real"), 3.14);
+
+  auto vect_out = bb_out->get<TestTypes::Vector3D>("vect");
+  ASSERT_EQ(vect_out.x, 1.1);
+  ASSERT_EQ(vect_out.y, 2.2);
+  ASSERT_EQ(vect_out.z, 3.3);
 }