Simplify the visualization of custom type in Groot2 and improved tutorial

Author: facontidavide

examples/t12_groot_howto.cpp

@@ -4,11 +4,48 @@
 #include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "behaviortree_cpp/loggers/bt_sqlite_logger.h"
 #include "behaviortree_cpp/xml_parsing.h"
+#include "behaviortree_cpp/json_export.h"
 
 /** We are using the same example in Tutorial 5,
  *  But this time we also show how to connect
  */
 
+// A custom structuree that I want to visualize in Groot2
+struct Position2D {
+  double x;
+  double y;
+};
+
+// Allows Position2D to be visualized in Groot2
+// You still need BT::RegisterJsonDefinition<Position2D>(PositionToJson)
+void PositionToJson(nlohmann::json& j, const Position2D& p)
+{
+  j["x"] = p.x;
+  j["y"] = p.y;
+}
+
+// Simple Action that updates an instance of Position2D in the blackboard
+class UpdatePosition: public BT::SyncActionNode
+{
+public:
+  UpdatePosition(const std::string& name, const BT::NodeConfig& config):
+    BT::SyncActionNode(name, config) {}
+
+  BT::NodeStatus tick() override {
+    _pos.x += 0.2;
+    _pos.y += 0.1;
+    setOutput("pos", _pos);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {BT::OutputPort<Position2D>("pos")};
+  }
+private:
+  Position2D _pos = {0, 0};
+};
+
 // clang-format off
 
 static const char* xml_text = R"(
@@ -17,6 +54,7 @@ static const char* xml_text = R"(
   <BehaviorTree ID="MainTree">
     <Sequence>
       <Script code="door_open:=false" />
+      <UpdatePosition pos="{pos_2D}" />
       <Fallback>
         <Inverter>
           <IsDoorClosed/>
@@ -46,27 +84,33 @@ int main()
 {
   BT::BehaviorTreeFactory factory;
 
+  // Nodes registration, as usual
   CrossDoor cross_door;
   cross_door.registerNodes(factory);
+  factory.registerNodeType<UpdatePosition>("UpdatePosition");
 
   // Groot2 editor requires a model of your registered Nodes.
   // You don't need to write that by hand, it can be automatically
   // generated using the following command.
   std::string xml_models = BT::writeTreeNodesModelXML(factory);
 
   factory.registerBehaviorTreeFromText(xml_text);
-  auto tree = factory.createTree("MainTree");
 
+  // Add this to allow Groot2 to visualize your custom type
+  BT::RegisterJsonDefinition<Position2D>(PositionToJson);
+
+  auto tree = factory.createTree("MainTree");
 
   std::cout << "----------- XML file  ----------\n"
             << BT::WriteTreeToXML(tree, false, false)
             << "--------------------------------\n";
 
   // Connect the Groot2Publisher. This will allow Groot2 to
   // get the tree and poll status updates.
-  BT::Groot2Publisher publisher(tree);
+  const unsigned port = 1667;
+  BT::Groot2Publisher publisher(tree, port);
 
-  // Add also two logger which save the transitions into a file.
+  // Add two more loggers, to save the transitions into a file.
   // Both formats are compatible with Groot2
 
   // Lightweight serialization
@@ -83,5 +127,7 @@ int main()
     std::this_thread::sleep_for(std::chrono::milliseconds(2000));
   }
 
+  BT::JsonExporter::get().addConverter<Position2D>();
+
   return 0;
 }

include/behaviortree_cpp/json_export.h

@@ -19,7 +19,7 @@ namespace BT
 *
 *  Later, you MUST register this calling:
 *
-*   JsonExporter::get().addConverter<Foo>();
+*   RegisterJsonDefinition<Foo>();
 */
 
 class JsonExporter{
@@ -55,6 +55,14 @@ class JsonExporter{
     type_converters_.insert( {typeid(T), std::move(converter)} );
   }
 
+  template <typename T> void addConverter(std::function<void(nlohmann::json&, const T&)> func)
+  {
+    auto converter = [func](const BT::Any& entry, nlohmann::json& dst) {
+      func(dst, entry.cast<T>());
+    };
+    type_converters_.insert( {typeid(T), std::move(converter)} );
+  }
+
   /// Register directly your own converter.
   template <typename T>
   void addConverter(std::function<void(const T&, nlohmann::json&)> to_json)
@@ -69,9 +77,45 @@ class JsonExporter{
 
   using ToJonConverter = std::function<void(const BT::Any&, nlohmann::json&)>;
   std::unordered_map<std::type_index, ToJonConverter> type_converters_;
-
 };
 
+/* Function to use to register a specific implementation of nlohmann::to_json
+
+  Example:
+
+  namespace nlohmann {
+    void to_json(nlohmann::json& j, const Position2D& p)
+    {
+      j["x"] = p.x;
+      j["y"] = p.y;
+    }
+  } // namespace nlohmann
+
+  // In you main function
+  RegisterJsonDefinition<Position2D>()
+*/
+template <typename T> inline void RegisterJsonDefinition()
+{
+  JsonExporter::get().addConverter<T>();
+}
+
+/* Function to use to register a specific implementation of "to_json"
+
+  Example:
+
+  RegisterJsonDefinition([](nlohmann::json& j, const Position2D& p)
+    {
+      j["x"] = p.x;
+      j["y"] = p.y;
+    } );
+*/
+
+template <typename T> inline
+void RegisterJsonDefinition(std::function<void(nlohmann::json&, const T&)> func)
+{
+  JsonExporter::get().addConverter<T>(func);
+}
+
 nlohmann::json ExportBlackboardToJSON(BT::Blackboard& blackboard);
 
 } // namespace BT