add FileLogger2

Author: facontidavide

CMakeLists.txt

@@ -119,7 +119,7 @@ list(APPEND BT_SOURCE
     src/controls/while_do_else_node.cpp
 
     src/loggers/bt_cout_logger.cpp
-    src/loggers/bt_file_logger.cpp
+    src/loggers/bt_file_logger_v2.cpp
     src/loggers/bt_minitrace_logger.cpp
     src/loggers/bt_observer.cpp
 

examples/t12_groot_howto.cpp

@@ -1,3 +1,4 @@
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
 #include "crossdoor_nodes.h"
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/loggers/groot2_publisher.h"
@@ -56,6 +57,7 @@ int main()
   factory.registerBehaviorTreeFromText(xml_text);
   auto tree = factory.createTree("MainTree");
 
+
   std::cout << "----------- XML file  ----------\n"
             << BT::WriteTreeToXML(tree, false)
             << "--------------------------------\n";
@@ -64,12 +66,21 @@ int main()
   // get the tree and poll status updates.
   BT::Groot2Publisher publisher(tree);
 
+  // Add also two logger which save the transitions into a file.
+  // Both formats are compatible with Groot2
+
+  // Lightweight serialization
+  BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
+  // SQLite logger can save multiple sessions into the same database
+  bool append_to_database = true;
+  BT::SqliteLogger sqlite_logger(tree, "t12_logger2.btsql", append_to_database);
+
   while(1)
   {
     std::cout << "Start" << std::endl;
     cross_door.reset();
     tree.tickWhileRunning();
-    std::this_thread::sleep_for(std::chrono::milliseconds(3000));
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
   }
 
   return 0;

include/behaviortree_cpp/flatbuffers/bt_flatbuffer_helper.h

@@ -1,8 +1,7 @@
-#ifndef BT_FLATBUFFER_HELPER_H
-#define BT_FLATBUFFER_HELPER_H
+#pragma once
 
 #include "behaviortree_cpp/bt_factory.h"
-#include "BT_logger_generated.h"
+#include "behaviortree_cpp/flatbuffers/BT_logger_generated.h"
 
 namespace BT
 {
@@ -160,4 +159,3 @@ inline SerializedTransition SerializeTransition(uint16_t UID, Duration timestamp
 
 }   // namespace BT
 
-#endif   // BT_FLATBUFFER_HELPER_H

include/behaviortree_cpp/loggers/bt_file_logger.h

@@ -8,7 +8,9 @@
 
 namespace BT
 {
-class FileLogger : public StatusChangeLogger
+
+class [[deprecated("Use FileLogger2 instead")]]
+FileLogger : public StatusChangeLogger
 {
 public:
   FileLogger(const Tree& tree, const char* filename, uint16_t buffer_size = 10);

include/behaviortree_cpp/loggers/bt_file_logger_v2.h

@@ -0,0 +1,61 @@
+#pragma once
+#include <fstream>
+#include <deque>
+#include <array>
+#include <filesystem>
+#include "behaviortree_cpp/loggers/abstract_logger.h"
+
+namespace BT
+{
+/**
+ * @brief The FileLogger2 is a logger that saves the tree as
+ * XML and all the transitions. Data is written to file in
+ * a separate thread, to minimize latency.
+ *
+ * Format:
+ *
+ * - first 4 bytes: size of the XML string (N)
+ * - next N bytes: string containing the XML representing the tree.
+ * - next 8 bytes: first timestamp (microseconds since epoch)
+ * - next: each 8 bytes is a FileLogger2::Transition. See definition.
+ *
+ */
+class FileLogger2 : public StatusChangeLogger
+{
+  public:
+  FileLogger2(const Tree& tree, std::filesystem::path const& filepath);
+
+  virtual ~FileLogger2() override;
+
+  void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                NodeStatus status) override;
+
+  struct Transition
+  {
+    // 45 bits are enough for 35 million seconds (more than 1 year) relative to the first timestamp
+    uint64_t timestamp_usec : 45;
+    // if you have more than 64.000 nodes, you are doing something wrong :)
+    uint64_t node_uid : 16;
+    // enough bits to contain NodeStatus
+    uint64_t status : 3;
+  };
+
+  void flush() override;
+
+private:
+  std::ofstream file_stream_;
+
+  Duration first_timestamp_ = {};
+
+  std::deque<Transition> transitions_queue_;
+  std::condition_variable queue_cv_;
+  std::mutex queue_mutex_;
+
+  std::thread writer_thread_;
+  std::atomic_bool loop_ = true;
+
+  void writerLoop();
+};
+
+}   // namespace BT
+

include/behaviortree_cpp/loggers/bt_sqlite_logger.h

@@ -52,15 +52,14 @@ class SqliteLogger : public StatusChangeLogger
     NodeStatus status;
   };
 
-  std::deque<Transition> write_queue_;
-  std::condition_variable queue_push_cv_;
-  std::condition_variable queue_pop_cv_;
+  std::deque<Transition> transitions_queue_;
+  std::condition_variable queue_cv_;
   std::mutex queue_mutex_;
 
-  std::thread queue_thread_;
+  std::thread writer_thread_;
   std::atomic_bool loop_ = true;
 
-  void threadLoop();
+  void writerLoop();
 
 };
 

src/loggers/bt_file_logger_v2.cpp

@@ -0,0 +1,94 @@
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/flatbuffers/base.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+namespace BT
+{
+
+int64_t ToUsec(Duration ts)
+{
+  return std::chrono::duration_cast<std::chrono::microseconds>(ts).count();
+}
+
+FileLogger2::FileLogger2(const BT::Tree& tree, std::filesystem::path const& filepath) :
+  StatusChangeLogger(tree.rootNode())
+{
+  enableTransitionToIdle(true);
+
+  //-------------------------------------
+  file_stream_.open(filepath, std::ofstream::binary | std::ofstream::out);
+
+  std::string const xml = WriteTreeToXML(tree, true);
+
+  // serialize the length of the buffer in the first 4 bytes
+  char write_buffer[8];
+  flatbuffers::WriteScalar(write_buffer, static_cast<int32_t>(xml.size()));
+  file_stream_.write(write_buffer, 4);
+
+  // write the XML definition
+  file_stream_.write(xml.data(), int(xml.size()));
+
+  first_timestamp_ = std::chrono::system_clock::now().time_since_epoch();
+
+  // save the first timestamp in the next 8 bytes (microseconds)
+  int64_t timestamp_usec = ToUsec(first_timestamp_);
+  flatbuffers::WriteScalar(write_buffer, timestamp_usec);
+  file_stream_.write(write_buffer, 8);
+
+  writer_thread_ = std::thread(&FileLogger2::writerLoop, this);
+}
+
+FileLogger2::~FileLogger2()
+{
+  loop_ = false;
+  queue_cv_.notify_one();
+  writer_thread_.join();
+  file_stream_.close();
+}
+
+void FileLogger2::callback(Duration timestamp, const TreeNode& node,
+                          NodeStatus /*prev_status*/, NodeStatus status)
+{
+  Transition trans;
+  trans.timestamp_usec = uint64_t(ToUsec(timestamp - first_timestamp_));
+  trans.node_uid = node.UID();
+  trans.status = static_cast<uint64_t>(status);
+  {
+    std::scoped_lock lock(queue_mutex_);
+    transitions_queue_.push_back(trans);
+  }
+  queue_cv_.notify_one();
+}
+
+void FileLogger2::flush()
+{
+  file_stream_.flush();
+}
+
+void FileLogger2::writerLoop()
+{
+  // local buffer in this thread
+  std::deque<Transition> transitions;
+
+  while(loop_)
+  {
+    transitions.clear();
+    {
+      std::unique_lock lock(queue_mutex_);
+      queue_cv_.wait_for(lock, std::chrono::milliseconds(10),
+                         [this]() {return !transitions_queue_.empty() && loop_; } );
+      // simple way to pop all the transitions from transitions_queue_ into transitions
+      std::swap(transitions, transitions_queue_);
+    }
+    while(!transitions.empty())
+    {
+      char write_buffer[8];
+      flatbuffers::WriteScalar(write_buffer, transitions.front());
+      file_stream_.write(write_buffer, 8);
+      transitions.pop_front();
+    }
+    file_stream_.flush();
+  }
+}
+
+}   // namespace BT

src/loggers/bt_sqlite_logger.cpp

@@ -49,15 +49,15 @@ SqliteLogger::SqliteLogger(const Tree &tree,
   {
     session_id_ = res.Get(0);
   }
-  queue_thread_ = std::thread(&SqliteLogger::threadLoop, this);
+  writer_thread_ = std::thread(&SqliteLogger::writerLoop, this);
 }
 
 SqliteLogger::~SqliteLogger()
 {
-  flush();
   loop_ = false;
-  queue_push_cv_.notify_one();
-  queue_thread_.join();
+  queue_cv_.notify_one();
+  writer_thread_.join();
+  flush();
   sqlite::Statement(*db_, "PRAGMA optimize;");
 }
 
@@ -96,46 +96,46 @@ void SqliteLogger::callback(Duration timestamp,
 
   {
     std::scoped_lock lk(queue_mutex_);
-    write_queue_.push_back(trans);
+    transitions_queue_.push_back(trans);
   }
-  queue_push_cv_.notify_one();
+  queue_cv_.notify_one();
 }
 
-void SqliteLogger::threadLoop()
+void SqliteLogger::writerLoop()
 {
+  std::deque<Transition> transitions;
+
   while(loop_)
   {
-    std::unique_lock lk(queue_mutex_);
-    queue_push_cv_.wait(lk, [this]() {
-      return !write_queue_.empty() || !loop_;
-    });
-
-    if(!loop_) {
-      break;
+    transitions.clear();
+    {
+      std::unique_lock lk(queue_mutex_);
+      queue_cv_.wait(lk, [this]() {
+        return !transitions_queue_.empty() || !loop_;
+      });
+      std::swap(transitions, transitions_queue_);
     }
 
-    auto const trans = write_queue_.front();
-    write_queue_.pop_front();
-    lk.unlock();
-    queue_pop_cv_.notify_all();
-
-    sqlite::Statement(
-        *db_,
-        "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
-        trans.timestamp,
-        session_id_,
-        trans.node_uid,
-        trans.duration,
-        static_cast<int>(trans.status));
+    while(!transitions.empty())
+    {
+      auto const trans = transitions.front();
+      transitions.pop_front();
+
+      sqlite::Statement(
+          *db_,
+          "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
+          trans.timestamp,
+          session_id_,
+          trans.node_uid,
+          trans.duration,
+          static_cast<int>(trans.status));
+    }
   }
 }
 
 void BT::SqliteLogger::flush()
 {
-  std::unique_lock lk(queue_mutex_);
-  queue_pop_cv_.wait(lk, [&]() {
-    return write_queue_.empty();
-  });
+  sqlite3_db_cacheflush(db_->GetPtr());
 }
 
 }