Merge branch 'log_snapshot'

facontidavide · facontidavide · commit 3018e16a390a · 2024-04-22T15:45:06.000+02:00
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -39,6 +39,7 @@ CompileExample("ex04_waypoints")
 CompileExample("ex05_subtree_model")
 CompileExample("ex06_access_by_ptr")
 CompileExample("ex07_blackboard_backup")
+CompileExample("ex08_sqlite_log")
 
 CompileExample("t13_plugin_executor")
 
diff --git a/examples/ex08_sqlite_log.cpp b/examples/ex08_sqlite_log.cpp
@@ -0,0 +1,151 @@
+#include "dummy_nodes.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/bt_sqlite_logger.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+#include "behaviortree_cpp/json_export.h"
+
+struct TaskA
+{
+  int type;
+  std::string name;
+};
+
+struct TaskB
+{
+  double value;
+  std::string name;
+};
+
+using Command = std::variant<TaskA, TaskB>;
+
+// Simple Action that updates an instance of Position2D in the blackboard
+class SetTask : public BT::SyncActionNode
+{
+public:
+  SetTask(const std::string& name, const BT::NodeConfig& config)
+    : BT::SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick() override
+  {
+    auto type = getInput<std::string>("type").value();
+    if(type == "A")
+    {
+      setOutput<Command>("task", TaskA{ 43, type });
+    }
+    else if(type == "B")
+    {
+      setOutput<Command>("task", TaskB{ 3.14, type });
+    }
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return { BT::InputPort<std::string>("type"), BT::OutputPort<Command>("task") };
+  }
+
+private:
+};
+
+// clang-format off
+
+static const char* xml_text = R"(
+<root BTCPP_format="4">
+
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+
+      <Script code="type:='A'" />
+      <SetTask type="{type}" task="{task}" />
+      <SubTree ID="ExecuteTaskA" task="{task}" _skipIf=" type!='A' " />
+
+      <Script code="type:='B'" />
+      <SetTask type="{type}" task="{task}" />
+      <SubTree ID="ExecuteTaskB" task="{task}" _skipIf=" type!='B' " />
+
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="ExecuteTaskA">
+    <Sequence>
+      <Sleep msec="1000"/>
+      <SaySomething message="executed command A" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="ExecuteTaskB">
+    <Sequence>
+      <Sleep msec="1000"/>
+      <SaySomething message="executed command B" />
+    </Sequence>
+  </BehaviorTree>
+
+</root>
+ )";
+
+// clang-format on
+
+int main()
+{
+  BT::BehaviorTreeFactory factory;
+
+  // Nodes registration, as usual
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerNodeType<SetTask>("SetTask");
+
+  // 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");
+
+  std::cout << "----------- XML file  ----------\n"
+            << BT::WriteTreeToXML(tree, false, false)
+            << "--------------------------------\n";
+
+  BT::SqliteLogger sqlite_logger(tree, "ex08_sqlitelog.db3", false);
+
+  //------------------------------------------------------------------------
+  // Write some data (from the blackboard) and write into the
+  // extra column called "extra_data". We will use JSON serialization
+
+  auto meta_callback = [&](BT::Duration timestamp, const BT::TreeNode& node,
+                           BT::NodeStatus prev_status,
+                           BT::NodeStatus status) -> std::string {
+    if(prev_status == BT::NodeStatus::RUNNING && BT::isStatusCompleted(status))
+    {
+      if(node.name() == "ExecuteTaskA")
+      {
+        auto task = node.config().blackboard->get<Command>("task");
+        auto taskA = std::get<TaskA>(task);
+        nlohmann::json json;
+        json["taskA"] = { { "name", taskA.name }, { "type", taskA.type } };
+        return json.dump();
+      }
+      if(node.name() == "ExecuteTaskB")
+      {
+        auto task = node.config().blackboard->get<Command>("task");
+        auto taskB = std::get<TaskB>(task);
+        nlohmann::json json;
+        json["taskB"] = { { "name", taskB.name }, { "value", taskB.value } };
+        return json.dump();
+      }
+    }
+    return {};
+  };
+  sqlite_logger.setAdditionalCallback(meta_callback);
+  //------------------------------------------------------------------------
+  while(1)
+  {
+    std::cout << "Start" << std::endl;
+    tree.tickWhileRunning();
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+  }
+
+  return 0;
+}
diff --git a/examples/t12_groot_howto.cpp b/examples/t12_groot_howto.cpp
@@ -98,7 +98,7 @@ int main()
   // 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);
+  const std::string xml_models = BT::writeTreeNodesModelXML(factory);
 
   factory.registerBehaviorTreeFromText(xml_text);
 
@@ -125,6 +125,21 @@ int main()
   bool append_to_database = true;
   BT::SqliteLogger sqlite_logger(tree, "t12_sqlitelog.db3", append_to_database);
 
+  // We can add some extra information to the SqliteLogger, for instance the value of the
+  // "door_open" blackboard entry, at the end of node "tryOpen" (Fallback)
+
+  auto sqlite_callback = [](BT::Duration timestamp, const BT::TreeNode& node,
+                            BT::NodeStatus prev_status,
+                            BT::NodeStatus status) -> std::string {
+    if(node.name() == "tryOpen" && BT::isStatusCompleted(status))
+    {
+      auto is_open = BT::toStr(node.config().blackboard->get<bool>("door_open"));
+      return "[tryOpen] door_open=" + is_open;
+    }
+    return {};
+  };
+  sqlite_logger.setAdditionalCallback(sqlite_callback);
+
   while(1)
   {
     std::cout << "Start" << std::endl;
diff --git a/include/behaviortree_cpp/loggers/bt_sqlite_logger.h b/include/behaviortree_cpp/loggers/bt_sqlite_logger.h
@@ -11,6 +11,28 @@ class Connection;
 namespace BT
 {
 
+/** SQL schema
+ *
+ * CREATE TABLE IF NOT EXISTS Definitions (
+ *     session_id INTEGER PRIMARY KEY AUTOINCREMENT,
+ *     date       TEXT NOT NULL,
+ *     xml_tree   TEXT NOT NULL);
+ *
+ * CREATE TABLE IF NOT EXISTS Nodes ("
+ *     session_id INTEGER NOT NULL,
+ *     fullpath   VARCHAR, "
+ *     node_uid   INTEGER NOT NULL );
+ *
+ * CREATE TABLE IF NOT EXISTS Transitions (
+ *     timestamp  INTEGER PRIMARY KEY NOT NULL,
+ *     session_id INTEGER NOT NULL,
+ *     node_uid   INTEGER NOT NULL,
+ *     duration   INTEGER,
+ *     state      INTEGER NOT NULL,
+ *     extra_data VARCHAR );
+ *
+ */
+
 /**
  * @brief The SqliteLogger is a logger that will store the tree and all the
  * status transitions in a SQLite database (single file).
@@ -37,9 +59,18 @@ class SqliteLogger : public StatusChangeLogger
 
   virtual ~SqliteLogger() override;
 
+  // You can inject a function that add a string to the Transitions table,
+  // in the column "extra_data".
+  // The arguments of the function are the same as SqliteLogger::callback()
+  using ExtraCallback =
+      std::function<std::string(Duration, const TreeNode&, NodeStatus, NodeStatus)>;
+  void setAdditionalCallback(ExtraCallback func);
+
   virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
                         NodeStatus status) override;
 
+  void execSqlStatement(std::string statement);
+
   virtual void flush() override;
 
 private:
@@ -56,6 +87,7 @@ class SqliteLogger : public StatusChangeLogger
     int64_t timestamp;
     int64_t duration;
     NodeStatus status;
+    std::string extra_data;
   };
 
   std::deque<Transition> transitions_queue_;
@@ -65,6 +97,8 @@ class SqliteLogger : public StatusChangeLogger
   std::thread writer_thread_;
   std::atomic_bool loop_ = true;
 
+  ExtraCallback extra_func_;
+
   void writerLoop();
 };
 
diff --git a/src/loggers/bt_sqlite_logger.cpp b/src/loggers/bt_sqlite_logger.cpp
@@ -22,9 +22,15 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
   sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Transitions ("
                           "timestamp  INTEGER PRIMARY KEY NOT NULL, "
                           "session_id INTEGER NOT NULL, "
-                          "uid        INTEGER NOT NULL, "
+                          "node_uid   INTEGER NOT NULL, "
                           "duration   INTEGER, "
-                          "state      INTEGER NOT NULL);");
+                          "state      INTEGER NOT NULL,"
+                          "extra_data VARCHAR );");
+
+  sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Nodes ("
+                          "session_id INTEGER NOT NULL, "
+                          "fullpath   VARCHAR, "
+                          "node_uid   INTEGER NOT NULL );");
 
   sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Definitions ("
                           "session_id INTEGER PRIMARY KEY AUTOINCREMENT, "
@@ -35,6 +41,7 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
   {
     sqlite::Statement(*db_, "DELETE from Transitions;");
     sqlite::Statement(*db_, "DELETE from Definitions;");
+    sqlite::Statement(*db_, "DELETE from Nodes;");
   }
 
   auto tree_xml = WriteTreeToXML(tree, true, true);
@@ -43,12 +50,23 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
                     "VALUES (datetime('now','localtime'),?);",
                     tree_xml);
 
-  auto res = sqlite::Query(*db_, "SELECT MAX(session_id) FROM Definitions LIMIT 1;");
+  auto res = sqlite::Query(*db_, "SELECT MAX(session_id) "
+                                 "FROM Definitions LIMIT 1;");
 
   while(res.Next())
   {
     session_id_ = res.Get(0);
   }
+
+  for(const auto& subtree : tree.subtrees)
+  {
+    for(const auto& node : subtree->nodes)
+    {
+      sqlite::Statement(*db_, "INSERT INTO Nodes VALUES (?, ?, ?)", session_id_,
+                        node->fullPath(), node->UID());
+    }
+  }
+
   writer_thread_ = std::thread(&SqliteLogger::writerLoop, this);
 }
 
@@ -61,6 +79,11 @@ SqliteLogger::~SqliteLogger()
   sqlite::Statement(*db_, "PRAGMA optimize;");
 }
 
+void SqliteLogger::setAdditionalCallback(ExtraCallback func)
+{
+  extra_func_ = func;
+}
+
 void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
                             NodeStatus prev_status, NodeStatus status)
 {
@@ -91,11 +114,26 @@ void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
   trans.node_uid = node.UID();
   trans.status = status;
 
+  if(extra_func_)
+  {
+    trans.extra_data = extra_func_(timestamp, node, prev_status, status);
+  }
+
   {
     std::scoped_lock lk(queue_mutex_);
     transitions_queue_.push_back(trans);
   }
   queue_cv_.notify_one();
+
+  if(extra_func_)
+  {
+    extra_func_(timestamp, node, prev_status, status);
+  }
+}
+
+void SqliteLogger::execSqlStatement(std::string statement)
+{
+  sqlite::Statement(*db_, statement);
 }
 
 void SqliteLogger::writerLoop()
@@ -116,9 +154,9 @@ void SqliteLogger::writerLoop()
       auto const trans = transitions.front();
       transitions.pop_front();
 
-      sqlite::Statement(*db_, "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
+      sqlite::Statement(*db_, "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?, ?)",
                         trans.timestamp, session_id_, trans.node_uid, trans.duration,
-                        static_cast<int>(trans.status));
+                        static_cast<int>(trans.status), trans.extra_data);
     }
   }
 }
