diff --git a/include/behaviortree_cpp/json_export.h b/include/behaviortree_cpp/json_export.h index 1d47b0877..3eeee714b 100644 --- a/include/behaviortree_cpp/json_export.h +++ b/include/behaviortree_cpp/json_export.h @@ -11,7 +11,7 @@ namespace BT { /** -* To add new type to the JSON library, you should follow these isntructions: +* To add new type to the JSON library, you should follow these instructions: * https://json.nlohmann.me/features/arbitrary_types/ * * Considering for instance the type: @@ -80,22 +80,31 @@ class JsonExporter template Expected fromJson(const nlohmann::json& source) const; - /// Register new JSON converters with addConverter(). - /// You should have used first the macro BT_JSON_CONVERTER + /** + * @brief Register new JSON converters with addConverter(). + * You should used first the macro BT_JSON_CONVERTER. + * The convertions from/to vector are automatically registered. + */ template void addConverter(); /** * @brief addConverter register a to_json function that converts a json to a type T. + * The convertion to std:vector is automatically registered. * * @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 void addConverter(std::function to_json, bool add_type = true); - /// Register custom from_json converter directly. + /** + * @brief addConverter register a from_json function that converts a json to a type T. + * The convertions from std::vector is automatically registered. + * + * @param from_json the function with signature void(const nlohmann::json&, T&) + */ template void addConverter(std::function from_json); @@ -105,6 +114,7 @@ class JsonExporter std::unordered_map to_json_converters_; std::unordered_map from_json_converters_; + std::unordered_map from_json_array_converters_; std::unordered_map type_names_; }; @@ -129,6 +139,15 @@ inline Expected JsonExporter::fromJson(const nlohmann::json& source) const template inline void JsonExporter::addConverter() { + // we need to get the name of the type + nlohmann::json const js = T{}; + // we insert both the name obtained from JSON and demangle + if(js.contains("__type")) + { + type_names_.insert({ std::string(js["__type"]), BT::TypeInfo::Create() }); + } + type_names_.insert({ BT::demangle(typeid(T)), BT::TypeInfo::Create() }); + ToJonConverter to_converter = [](const BT::Any& entry, nlohmann::json& dst) { dst = *const_cast(entry).castPtr(); }; @@ -139,16 +158,23 @@ inline void JsonExporter::addConverter() return { BT::Any(value), BT::TypeInfo::Create() }; }; - // we need to get the name of the type - nlohmann::json const js = T{}; - // we insert both the name obtained from JSON and demangle - if(js.contains("__type")) - { - type_names_.insert({ std::string(js["__type"]), BT::TypeInfo::Create() }); - } - type_names_.insert({ BT::demangle(typeid(T)), BT::TypeInfo::Create() }); - from_json_converters_.insert({ typeid(T), from_converter }); + + //---- include vectors of T + ToJonConverter to_array_converter = [](const BT::Any& entry, nlohmann::json& dst) { + dst = *const_cast(entry).castPtr>(); + }; + to_json_converters_.insert({ typeid(std::vector), to_array_converter }); + + FromJonConverter from_array_converter = [](const nlohmann::json& src) -> Entry { + std::vector value; + for(const auto& item : src) + { + value.push_back(item.get()); + } + return { BT::Any(value), BT::TypeInfo::Create>() }; + }; + from_json_array_converters_.insert({ typeid(T), from_array_converter }); } template @@ -163,6 +189,18 @@ inline void JsonExporter::addConverter( } }; to_json_converters_.insert({ typeid(T), std::move(converter) }); + //--------------------------------------------- + // add the vector converter + auto vector_converter = [converter](const BT::Any& entry, nlohmann::json& json) { + auto& vec = *const_cast(entry).castPtr>(); + for(const auto& item : vec) + { + nlohmann::json item_json; + converter(BT::Any(item), item_json); + json.push_back(item_json); + } + }; + to_json_converters_.insert({ typeid(std::vector), std::move(vector_converter) }); } template @@ -176,6 +214,19 @@ JsonExporter::addConverter(std::function func) }; type_names_.insert({ BT::demangle(typeid(T)), BT::TypeInfo::Create() }); from_json_converters_.insert({ typeid(T), std::move(converter) }); + //--------------------------------------------- + // add the vector converter + auto vector_converter = [func](const nlohmann::json& json) -> Entry { + std::vector tmp; + for(const auto& item : json) + { + T item_tmp; + func(item, item_tmp); + tmp.push_back(item_tmp); + } + return { BT::Any(tmp), BT::TypeInfo::Create>() }; + }; + from_json_array_converters_.insert({ typeid(T), std::move(vector_converter) }); } template diff --git a/include/behaviortree_cpp/utils/safe_any.hpp b/include/behaviortree_cpp/utils/safe_any.hpp index 999e7d526..e056394ac 100644 --- a/include/behaviortree_cpp/utils/safe_any.hpp +++ b/include/behaviortree_cpp/utils/safe_any.hpp @@ -162,32 +162,7 @@ class Any [[nodiscard]] T* castPtr() { static_assert(!std::is_same_v, "The value has been casted internally to " - "[double]. " - "Use that instead"); - static_assert(!SafeAny::details::is_integer() || std::is_same_v, "The" - " va" - "lue" - " ha" - "s " - "bee" - "n " - "cas" - "ted" - " in" - "ter" - "nal" - "ly " - "to " - "[in" - "t64" - "_t]" - ". " - "Use" - " th" - "at " - "ins" - "tea" - "d"); + "[double]. Use that instead"); return _any.empty() ? nullptr : linb::any_cast(&_any); } diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index af70a94c5..36cf029b1 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -13,7 +13,6 @@ #include #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/contrib/json.hpp" #include "behaviortree_cpp/xml_parsing.h" #include "wildcards/wildcards.hpp" diff --git a/src/json_export.cpp b/src/json_export.cpp index 1baac478a..2ad648f3e 100644 --- a/src/json_export.cpp +++ b/src/json_export.cpp @@ -30,6 +30,22 @@ bool JsonExporter::toJson(const Any& any, nlohmann::json& dst) const { dst = any.cast(); } + else if(type == typeid(std::vector)) + { + dst = any.cast>(); + } + else if(type == typeid(std::vector)) + { + dst = any.cast>(); + } + else if(type == typeid(std::vector)) + { + dst = any.cast>(); + } + else if(type == typeid(std::vector)) + { + dst = any.cast>(); + } else { auto it = to_json_converters_.find(type); @@ -49,20 +65,17 @@ JsonExporter::ExpectedEntry JsonExporter::fromJson(const nlohmann::json& source) { if(source.is_null()) { - return nonstd::make_unexpected("json object is null"); + return Entry{ BT::Any(), BT::TypeInfo::Create() }; } + if(source.is_string()) { return Entry{ BT::Any(source.get()), BT::TypeInfo::Create() }; } - if(source.is_number_unsigned()) - { - return Entry{ BT::Any(source.get()), BT::TypeInfo::Create() }; - } if(source.is_number_integer()) { - return Entry{ BT::Any(source.get()), BT::TypeInfo::Create() }; + return Entry{ BT::Any(source.get()), BT::TypeInfo::Create() }; } if(source.is_number_float()) { @@ -73,17 +86,48 @@ JsonExporter::ExpectedEntry JsonExporter::fromJson(const nlohmann::json& source) return Entry{ BT::Any(source.get()), BT::TypeInfo::Create() }; } - if(!source.contains("__type")) + // basic vectors + if(source.is_array() && source.size() > 0 && !source.contains("__type")) + { + auto first_element = source[0]; + if(first_element.is_string()) + { + return Entry{ BT::Any(source.get>()), + BT::TypeInfo::Create>() }; + } + if(first_element.is_number_integer()) + { + return Entry{ BT::Any(source.get>()), + BT::TypeInfo::Create>() }; + } + if(first_element.is_number_float()) + { + return Entry{ BT::Any(source.get>()), + BT::TypeInfo::Create>() }; + } + if(first_element.is_boolean()) + { + return Entry{ BT::Any(source.get>()), + BT::TypeInfo::Create>() }; + } + } + + if(!source.contains("__type") && !source.is_array()) { return nonstd::make_unexpected("Missing field '__type'"); } - auto type_it = type_names_.find(source["__type"]); + + auto& from_converters = + source.is_array() ? from_json_array_converters_ : from_json_converters_; + auto type_field = source.is_array() ? source[0]["__type"] : source["__type"]; + + auto type_it = type_names_.find(type_field); if(type_it == type_names_.end()) { return nonstd::make_unexpected("Type not found in registered list"); } - auto func_it = from_json_converters_.find(type_it->second.type()); - if(func_it == from_json_converters_.end()) + auto func_it = from_converters.find(type_it->second.type()); + if(func_it == from_converters.end()) { return nonstd::make_unexpected("Type not found in registered list"); } diff --git a/tests/gtest_json.cpp b/tests/gtest_json.cpp index f7b9e5909..24bba62d9 100644 --- a/tests/gtest_json.cpp +++ b/tests/gtest_json.cpp @@ -13,6 +13,10 @@ struct Vector3D double x = 0; double y = 0; double z = 0; + bool operator==(const Vector3D& other) const + { + return x == other.x && y == other.y && z == other.z; + } }; struct Quaternion3D @@ -21,12 +25,20 @@ struct Quaternion3D double x = 0; double y = 0; double z = 0; + bool operator==(const Quaternion3D& other) const + { + return w == other.w && x == other.x && y == other.y && z == other.z; + } }; struct Pose3D { Vector3D pos; Quaternion3D rot; + bool operator==(const Pose3D& other) const + { + return pos == other.pos && rot == other.rot; + } }; struct Time @@ -114,22 +126,21 @@ TEST_F(JsonTest, TwoWaysConversion) ASSERT_EQ(json["pose"]["rot"]["y"], 6); ASSERT_EQ(json["pose"]["rot"]["z"], 7); - // check the two-ways transform, i.e. "from_json" - auto pose2 = exporter.fromJson(json["pose"])->first.cast(); - - ASSERT_EQ(pose.pos.x, pose2.pos.x); - ASSERT_EQ(pose.pos.y, pose2.pos.y); - ASSERT_EQ(pose.pos.z, pose2.pos.z); - - ASSERT_EQ(pose.rot.w, pose2.rot.w); - ASSERT_EQ(pose.rot.x, pose2.rot.x); - ASSERT_EQ(pose.rot.y, pose2.rot.y); - ASSERT_EQ(pose.rot.z, pose2.rot.z); - - auto num = exporter.fromJson(json["int"])->first.cast(); + auto num_result = exporter.fromJson(json["int"]); + ASSERT_TRUE(num_result) << num_result.error(); + auto num = num_result->first.cast(); ASSERT_EQ(num, 69); - auto real = exporter.fromJson(json["real"])->first.cast(); + + auto real_result = exporter.fromJson(json["real"]); + ASSERT_TRUE(real_result) << real_result.error(); + auto real = real_result->first.cast(); ASSERT_EQ(real, 3.14); + + auto pose_result = exporter.fromJson(json["pose"]); + ASSERT_TRUE(pose_result) << pose_result.error(); + auto pose_out = pose_result->first.cast(); + ASSERT_EQ(pose.pos, pose_out.pos); + ASSERT_EQ(pose.rot, pose_out.rot); } TEST_F(JsonTest, CustomTime) @@ -195,3 +206,103 @@ TEST_F(JsonTest, BlackboardInOut) ASSERT_EQ(vect_out.y, 2.2); ASSERT_EQ(vect_out.z, 3.3); } + +TEST_F(JsonTest, VectorInteger) +{ + BT::JsonExporter& exporter = BT::JsonExporter::get(); + + std::vector vec = { 1, 2, 3 }; + nlohmann::json json; + exporter.toJson(BT::Any(vec), json["vec"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["vec"][0], 1); + ASSERT_EQ(json["vec"][1], 2); + ASSERT_EQ(json["vec"][2], 3); + + auto result = exporter.fromJson(json["vec"]); + ASSERT_TRUE(result) << result.error(); + auto vec_out = result->first.cast>(); + + ASSERT_EQ(vec.size(), vec_out.size()); + for(size_t i = 0; i < vec.size(); ++i) + { + ASSERT_EQ(vec[i], vec_out[i]); + } +} + +TEST_F(JsonTest, VectorSring) +{ + BT::JsonExporter& exporter = BT::JsonExporter::get(); + std::vector vec = { "hello", "world" }; + nlohmann::json json; + exporter.toJson(BT::Any(vec), json["vec"]); + std::cout << json.dump(2) << std::endl; + ASSERT_EQ(json["vec"][0], "hello"); + ASSERT_EQ(json["vec"][1], "world"); + auto result = exporter.fromJson(json["vec"]); + ASSERT_TRUE(result) << result.error(); + auto vec_out = result->first.cast>(); + ASSERT_EQ(vec.size(), vec_out.size()); + for(size_t i = 0; i < vec.size(); ++i) + { + ASSERT_EQ(vec[i], vec_out[i]); + } + // check the two-ways transform, i.e. "from_json" + auto result2 = exporter.fromJson(json["vec"]); + ASSERT_TRUE(result2) << result2.error(); + auto vec_out2 = result2->first.cast>(); + ASSERT_EQ(vec.size(), vec_out2.size()); + for(size_t i = 0; i < vec.size(); ++i) + { + ASSERT_EQ(vec[i], vec_out2[i]); + } +} + +TEST_F(JsonTest, VectorOfCustomTypes) +{ + BT::JsonExporter& exporter = BT::JsonExporter::get(); + + std::vector poses(2); + poses[0].pos = { 1, 2, 3 }; + poses[0].rot = { 4, 5, 6, 7 }; + + poses[1].pos = { 8, 9, 10 }; + poses[1].rot = { 11, 12, 13, 14 }; + + nlohmann::json json; + exporter.toJson(BT::Any(poses), json["poses"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["poses"][0]["__type"], "Pose3D"); + ASSERT_EQ(json["poses"][0]["pos"]["x"], 1); + ASSERT_EQ(json["poses"][0]["pos"]["y"], 2); + ASSERT_EQ(json["poses"][0]["pos"]["z"], 3); + ASSERT_EQ(json["poses"][0]["rot"]["w"], 4); + ASSERT_EQ(json["poses"][0]["rot"]["x"], 5); + ASSERT_EQ(json["poses"][0]["rot"]["y"], 6); + ASSERT_EQ(json["poses"][0]["rot"]["z"], 7); + ASSERT_EQ(json["poses"][1]["__type"], "Pose3D"); + ASSERT_EQ(json["poses"][1]["pos"]["x"], 8); + ASSERT_EQ(json["poses"][1]["pos"]["y"], 9); + ASSERT_EQ(json["poses"][1]["pos"]["z"], 10); + ASSERT_EQ(json["poses"][1]["rot"]["w"], 11); + ASSERT_EQ(json["poses"][1]["rot"]["x"], 12); + ASSERT_EQ(json["poses"][1]["rot"]["y"], 13); + ASSERT_EQ(json["poses"][1]["rot"]["z"], 14); + + // check the two-ways transform, i.e. "from_json" + auto result = exporter.fromJson(json["poses"]); + ASSERT_TRUE(result) << result.error(); + auto poses_out = result->first.cast>(); + + ASSERT_EQ(poses.size(), poses_out.size()); + + ASSERT_EQ(poses[0].pos, poses_out[0].pos); + ASSERT_EQ(poses[0].rot, poses_out[0].rot); + + ASSERT_EQ(poses[1].pos, poses_out[1].pos); + ASSERT_EQ(poses[1].rot, poses_out[1].rot); +}