add more informative IDLE status

Author: facontidavide

include/behaviortree_cpp/loggers/groot2_publisher.h

@@ -58,8 +58,9 @@ class Groot2Publisher : public StatusChangeLogger
 
   std::mutex status_mutex_;
 
-  std::unordered_map<uint16_t, char*> buffer_ptr_;
   std::string status_buffer_;
+  // each element of this map points to a character in status_buffer_
+  std::unordered_map<uint16_t, char*> status_buffer_map_;
 
   // weak reference to the tree.
   std::unordered_map<std::string, std::weak_ptr<BT::Tree::Subtree>> subtrees_;
@@ -73,6 +74,12 @@ class Groot2Publisher : public StatusChangeLogger
 
   std::thread heartbeat_thread_;
 
+  enum {
+    IDLE_FROM_SUCCESS = 10 + static_cast<int>(NodeStatus::SUCCESS),
+    IDLE_FROM_FAILURE = 10 + static_cast<int>(NodeStatus::FAILURE),
+    IDLE_FROM_RUNNING = 10 + static_cast<int>(NodeStatus::RUNNING)
+  };
+
   struct Pimpl;
   Pimpl* zmq_;
   void enableAllHooks(bool enable);

src/loggers/groot2_publisher.cpp

@@ -98,7 +98,7 @@ Groot2Publisher::Groot2Publisher(const BT::Tree& tree,
 
       ptr_offset += Monitor::Serialize(buffer_ptr, ptr_offset,
                                        node->UID());
-      buffer_ptr_.insert( {node->UID(), buffer_ptr + ptr_offset} );
+      status_buffer_map_.insert( {node->UID(), buffer_ptr + ptr_offset} );
       ptr_offset += Monitor::Serialize(buffer_ptr, ptr_offset,
                                        uint8_t(NodeStatus::IDLE));
     }
@@ -139,10 +139,15 @@ Groot2Publisher::~Groot2Publisher()
 }
 
 void Groot2Publisher::callback(Duration, const TreeNode& node,
-                               NodeStatus, NodeStatus status)
+                               NodeStatus prev_status, NodeStatus new_status)
 {
   std::unique_lock<std::mutex> lk(status_mutex_);
-  *(buffer_ptr_.at(node.UID())) = static_cast<char>(status);
+  auto status = static_cast<char>(new_status);
+
+  if( new_status == NodeStatus::IDLE) {
+    status = 10 + static_cast<char>(prev_status);
+  }
+  *(status_buffer_map_.at(node.UID())) = status;
 }
 
 void Groot2Publisher::flush()