add recording to groot publisher

Author: facontidavide

include/behaviortree_cpp/loggers/groot2_protocol.h

@@ -49,6 +49,11 @@ enum RequestType : uint8_t
 
   DISABLE_ALL_HOOKS = 'X',
 
+  // start/stop recordong
+  TOGGLE_RECORDING = 'r',
+  // get all transitions when recording
+  GET_TRANSITIONS = 't',
+
   UNDEFINED = 0,
 };
 
@@ -67,13 +72,15 @@ inline const char* ToString(const RequestType& type)
   case RequestType::REMOVE_ALL_HOOKS: return "hooks_remove_all";
   case RequestType::HOOKS_DUMP: return "hooks_dump";
   case RequestType::DISABLE_ALL_HOOKS: return "disable_hooks";
+  case RequestType::TOGGLE_RECORDING: return "toggle_recording";
+  case RequestType::GET_TRANSITIONS: return "get_transitions";
 
   case RequestType::UNDEFINED: return "undefined";
   }
   return "undefined";
 }
 
-constexpr uint8_t kProtocolID = 1;
+constexpr uint8_t kProtocolID = 2;
 using TreeUniqueUUID = std::array<char, 16>;
 
 struct RequestHeader

src/loggers/groot2_publisher.cpp

@@ -18,6 +18,16 @@ enum {
   IDLE_FROM_RUNNING = 10 + static_cast<int>(NodeStatus::RUNNING)
 };
 
+struct Transition
+{
+  // when serializing, we will remove the initial time and serialize only
+  // 6 bytes, instead of 8
+  uint64_t timestamp_usec;
+  // if you have more than 64.000 nodes, you are doing something wrong :)
+  uint16_t node_uid;
+  // enough bits to contain NodeStatus
+  uint8_t status;
+};
 
 std::array<char,16> CreateRandomUUID()
 {
@@ -82,6 +92,9 @@ struct Groot2Publisher::PImpl
   std::chrono::system_clock::time_point last_heartbeat;
   std::chrono::milliseconds max_heartbeat_delay = std::chrono::milliseconds(5000);
 
+  std::atomic_bool recording = false;
+  std::deque<Transition> transitions_buffer;
+
   std::thread heartbeat_thread;
 
   zmq::context_t context;
@@ -183,7 +196,7 @@ Groot2Publisher::~Groot2Publisher()
   }
 }
 
-void Groot2Publisher::callback(Duration, const TreeNode& node,
+void Groot2Publisher::callback(Duration ts, const TreeNode& node,
                                NodeStatus prev_status, NodeStatus new_status)
 {
   std::unique_lock<std::mutex> lk(_p->status_mutex);
@@ -193,6 +206,18 @@ void Groot2Publisher::callback(Duration, const TreeNode& node,
     status = 10 + static_cast<char>(prev_status);
   }
   *(_p->status_buffermap.at(node.UID())) = status;
+  if(_p->recording)
+  {
+    Transition trans;
+    trans.node_uid = node.UID();
+    trans.status = static_cast<uint8_t>(new_status);
+    trans.timestamp_usec =
+        std::chrono::duration_cast<std::chrono::microseconds>(ts).count();
+    _p->transitions_buffer.push_back(trans);
+    while(_p->transitions_buffer.size() > 1000) {
+      _p->transitions_buffer.pop_front();
+    }
+  }
 }
 
 void Groot2Publisher::flush()
@@ -383,6 +408,43 @@ void Groot2Publisher::serverLoop()
         }
         reply_msg.addstr( json_out.dump() );
       } break;
+
+      case Monitor::RequestType::TOGGLE_RECORDING:
+      {
+        if(requestMsg.size() != 2) {
+          sendErrorReply("must be 2 parts message");
+          continue;
+        }
+
+        auto const cmd = (requestMsg[1].to_string());
+        if(cmd == "start")
+        {
+          _p->recording = true;
+          std::unique_lock<std::mutex> lk(_p->status_mutex);
+          _p->transitions_buffer.clear();
+        }
+        else if(cmd == "stop")
+        {
+          _p->recording = false;
+        }
+      } break;
+
+      case Monitor::RequestType::GET_TRANSITIONS:
+      {
+        thread_local std::string trans_buffer;
+        const size_t N = sizeof(Transition);
+        trans_buffer.resize(N * _p->transitions_buffer.size());
+
+        std::unique_lock<std::mutex> lk(_p->status_mutex);
+        size_t offset = 0;
+        for(const auto& trans: _p->transitions_buffer)
+        {
+          std::memcpy(&trans_buffer[offset], &trans, N);
+          offset += N;
+        }
+        _p->transitions_buffer.clear();
+      } break;
+
       default: {
         sendErrorReply("Request not recognized");
         continue;