From 52d6d8d771ef2e8084bffc8f2521832025e39616 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Tue, 18 Jun 2024 23:15:50 +0200
Subject: [PATCH] fix issue #829: support again custom JSON converters

---
 include/behaviortree_cpp/json_export.h | 65 +++++++++++++++++++++++++-
 tests/gtest_json.cpp                   | 53 +++++++++++++++++++++
 2 files changed, 116 insertions(+), 2 deletions(-)

diff --git a/include/behaviortree_cpp/json_export.h b/include/behaviortree_cpp/json_export.h
index b478a8f64..11c3b4f86 100644
--- a/include/behaviortree_cpp/json_export.h
+++ b/include/behaviortree_cpp/json_export.h
@@ -2,7 +2,7 @@
 
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/utils/safe_any.hpp"
-#include "behaviortree_cpp/contrib/expected.hpp"
+#include "behaviortree_cpp/basic_types.h"
 
 // Use the version nlohmann::json embedded in BT.CPP
 #include "behaviortree_cpp/contrib/json.hpp"
@@ -73,14 +73,32 @@ class JsonExporter
    */
   ExpectedEntry fromJson(const nlohmann::json& source) const;
 
-  /// Same as the other, but providing the specific type
+  /// Same as the other, but providing the specific type,
+  /// To be preferred if the JSON doesn't contain the field [__type]
   ExpectedEntry fromJson(const nlohmann::json& source, std::type_index type) const;
 
+  template <typename T>
+  Expected<T> fromJson(const nlohmann::json& source) const;
+
   /// Register new JSON converters with addConverter<Foo>().
   /// You should have used first the macro BT_JSON_CONVERTER
   template <typename T>
   void addConverter();
 
+  /**
+   * @brief addConverter register a to_json function that converts a json to a type T.
+   *
+   * @param to_json the function with signature void(const T&, nlohmann::json&)
+   * @param add_type if true, add a field called [__type] with the name ofthe type.
+   * */
+  template <typename T>
+  void addConverter(std::function<void(const T&, nlohmann::json&)> to_json,
+                    bool add_type = true);
+
+  /// Register custom from_json converter directly.
+  template <typename T>
+  void addConverter(std::function<void(const nlohmann::json&, T&)> from_json);
+
 private:
   using ToJonConverter = std::function<void(const BT::Any&, nlohmann::json&)>;
   using FromJonConverter = std::function<Entry(const nlohmann::json&)>;
@@ -90,6 +108,22 @@ class JsonExporter
   std::unordered_map<std::string, BT::TypeInfo> type_names_;
 };
 
+template <typename T>
+inline Expected<T> JsonExporter::fromJson(const nlohmann::json& source) const
+{
+  auto res = fromJson(source);
+  if(!res)
+  {
+    return nonstd::expected_lite::make_unexpected(res.error());
+  }
+  auto casted = res->first.tryCast<T>();
+  if(!casted)
+  {
+    return nonstd::expected_lite::make_unexpected(casted.error());
+  }
+  return *casted;
+}
+
 //-------------------------------------------------------------------
 
 template <typename T>
@@ -117,6 +151,33 @@ inline void JsonExporter::addConverter()
   from_json_converters_.insert({ typeid(T), from_converter });
 }
 
+template <typename T>
+inline void JsonExporter::addConverter(
+    std::function<void(const T&, nlohmann::json&)> func, bool add_type)
+{
+  auto converter = [func, add_type](const BT::Any& entry, nlohmann::json& json) {
+    func(entry.cast<T>(), json);
+    if(add_type)
+    {
+      json["__type"] = BT::demangle(typeid(T));
+    }
+  };
+  to_json_converters_.insert({ typeid(T), std::move(converter) });
+}
+
+template <typename T>
+inline void
+JsonExporter::addConverter(std::function<void(const nlohmann::json&, T&)> func)
+{
+  auto converter = [func](const nlohmann::json& json) -> Entry {
+    T tmp;
+    func(json, tmp);
+    return { BT::Any(tmp), BT::TypeInfo::Create<T>() };
+  };
+  type_names_.insert({ BT::demangle(typeid(T)), BT::TypeInfo::Create<T>() });
+  from_json_converters_.insert({ typeid(T), std::move(converter) });
+}
+
 template <typename T>
 inline void RegisterJsonDefinition()
 {
diff --git a/tests/gtest_json.cpp b/tests/gtest_json.cpp
index f9b392fb1..f7b9e5909 100644
--- a/tests/gtest_json.cpp
+++ b/tests/gtest_json.cpp
@@ -29,6 +29,12 @@ struct Pose3D
   Quaternion3D rot;
 };
 
+struct Time
+{
+  uint32_t sec;
+  uint32_t nsec;
+};
+
 BT_JSON_CONVERTER(Vector3D, v)
 {
   add_field("x", &v.x);
@@ -50,6 +56,19 @@ BT_JSON_CONVERTER(Pose3D, v)
   add_field("rot", &v.rot);
 }
 
+// specialized functions
+void jsonFromTime(const Time& t, nlohmann::json& j)
+{
+  j["stamp"] = double(t.sec) + 1e-9 * double(t.nsec);
+}
+
+void jsonToTime(const nlohmann::json& j, Time& t)
+{
+  double sec = j["stamp"];
+  t.sec = int(sec);
+  t.nsec = (sec - t.sec) * 1e9;
+}
+
 }  // namespace TestTypes
 
 //----------- JSON specialization ----------
@@ -63,6 +82,9 @@ class JsonTest : public testing::Test
     exporter.addConverter<TestTypes::Pose3D>();
     exporter.addConverter<TestTypes::Vector3D>();
     exporter.addConverter<TestTypes::Quaternion3D>();
+
+    exporter.addConverter<TestTypes::Time>(TestTypes::jsonFromTime);
+    exporter.addConverter<TestTypes::Time>(TestTypes::jsonToTime);
   }
 };
 
@@ -110,6 +132,37 @@ TEST_F(JsonTest, TwoWaysConversion)
   ASSERT_EQ(real, 3.14);
 }
 
+TEST_F(JsonTest, CustomTime)
+{
+  BT::JsonExporter& exporter = BT::JsonExporter::get();
+
+  TestTypes::Time stamp = { 3, 8000000 };
+  nlohmann::json json;
+  exporter.toJson(BT::Any(stamp), json);
+  std::cout << json.dump() << std::endl;
+
+  {
+    auto res = exporter.fromJson(json, typeid(TestTypes::Time));
+    ASSERT_TRUE(res);
+    auto stamp_out = res->first.cast<TestTypes::Time>();
+    ASSERT_EQ(stamp.sec, stamp_out.sec);
+    ASSERT_EQ(stamp.nsec, stamp_out.nsec);
+  }
+  {
+    auto res = exporter.fromJson(json);
+    ASSERT_TRUE(res);
+    auto stamp_out = res->first.cast<TestTypes::Time>();
+    ASSERT_EQ(stamp.sec, stamp_out.sec);
+    ASSERT_EQ(stamp.nsec, stamp_out.nsec);
+  }
+  {
+    auto stamp_out = exporter.fromJson<TestTypes::Time>(json);
+    ASSERT_TRUE(stamp_out);
+    ASSERT_EQ(stamp.sec, stamp_out->sec);
+    ASSERT_EQ(stamp.nsec, stamp_out->nsec);
+  }
+}
+
 TEST_F(JsonTest, ConvertFromString)
 {
   TestTypes::Vector3D vect;