add timestamp and sequence to blackboard entries

Author: facontidavide

include/behaviortree_cpp/basic_types.h

@@ -332,6 +332,14 @@ using Optional = nonstd::expected<T, std::string>;
  * */
 using Result = Expected<std::monostate>;
 
+struct Timestamp
+{
+  uint64_t seq = 0;
+  std::chrono::nanoseconds stamp = std::chrono::nanoseconds(0);
+};
+
+using ResultStamped = Expected<Timestamp>;
+
 [[nodiscard]] bool IsAllowedPortName(StringView str);
 
 class TypeInfo

include/behaviortree_cpp/blackboard.h

@@ -4,6 +4,7 @@
 #include <memory>
 #include <unordered_map>
 #include <mutex>
+#include <optional>
 
 #include "behaviortree_cpp/basic_types.h"
 #include "behaviortree_cpp/contrib/json.hpp"
@@ -40,8 +41,14 @@ class Blackboard
     StringConverter string_converter;
     mutable std::mutex entry_mutex;
 
+    uint64_t sequence_id = 0;
+    // timestamp since epoch
+    std::chrono::nanoseconds stamp = std::chrono::nanoseconds{ 0 };
+
     Entry(const TypeInfo& _info) : info(_info)
     {}
+
+    Entry& operator=(const Entry& other);
   };
 
   /** Use this static method to create an instance of the BlackBoard
@@ -75,12 +82,18 @@ class Blackboard
   template <typename T>
   [[nodiscard]] bool get(const std::string& key, T& value) const;
 
+  template <typename T>
+  std::optional<Timestamp> getStamped(const std::string& key, T& value) const;
+
   /**
    * Version of get() that throws if it fails.
    */
   template <typename T>
   [[nodiscard]] T get(const std::string& key) const;
 
+  template <typename T>
+  std::pair<T, Timestamp> getStamped(const std::string& key) const;
+
   /// Update the entry with the given key
   template <typename T>
   void set(const std::string& key, const T& value);
@@ -155,10 +168,7 @@ inline T Blackboard::get(const std::string& key) const
     }
     return any_ref.get()->cast<T>();
   }
-  else
-  {
-    throw RuntimeError("Blackboard::get() error. Missing key [", key, "]");
-  }
+  throw RuntimeError("Blackboard::get() error. Missing key [", key, "]");
 }
 
 inline void Blackboard::unset(const std::string& key)
@@ -203,6 +213,8 @@ inline void Blackboard::set(const std::string& key, const T& value)
     lock.lock();
 
     entry->value = new_value;
+    entry->sequence_id++;
+    entry->stamp = std::chrono::steady_clock::now().time_since_epoch();
   }
   else
   {
@@ -212,14 +224,15 @@ inline void Blackboard::set(const std::string& key, const T& value)
     std::scoped_lock scoped_lock(entry.entry_mutex);
 
     Any& previous_any = entry.value;
-
     Any new_value(value);
 
     // special case: entry exists but it is not strongly typed... yet
     if(!entry.info.isStronglyTyped())
     {
       // Use the new type to create a new entry that is strongly typed.
       entry.info = TypeInfo::Create<T>();
+      entry.sequence_id++;
+      entry.stamp = std::chrono::steady_clock::now().time_since_epoch();
       previous_any = std::move(new_value);
       return;
     }
@@ -273,6 +286,8 @@ inline void Blackboard::set(const std::string& key, const T& value)
       // copy only if the type is compatible
       new_value.copyInto(previous_any);
     }
+    entry.sequence_id++;
+    entry.stamp = std::chrono::steady_clock::now().time_since_epoch();
   }
 }
 
@@ -281,10 +296,47 @@ inline bool Blackboard::get(const std::string& key, T& value) const
 {
   if(auto any_ref = getAnyLocked(key))
   {
+    if(any_ref.get()->empty())
+    {
+      return false;
+    }
     value = any_ref.get()->cast<T>();
     return true;
   }
   return false;
 }
 
+template <typename T>
+inline std::optional<Timestamp> Blackboard::getStamped(const std::string& key,
+                                                       T& value) const
+{
+  if(auto entry = getEntry(key))
+  {
+    std::unique_lock lk(entry->entry_mutex);
+    if(entry->value.empty())
+    {
+      return std::nullopt;
+    }
+    value = entry->value.cast<T>();
+    return Timestamp{ entry->sequence_id, entry->stamp };
+  }
+  return std::nullopt;
+}
+
+template <typename T>
+inline std::pair<T, Timestamp> Blackboard::getStamped(const std::string& key) const
+{
+  if(auto entry = getEntry(key))
+  {
+    std::unique_lock lk(entry->entry_mutex);
+    if(entry->value.empty())
+    {
+      throw RuntimeError("Blackboard::get() error. Entry [", key,
+                         "] hasn't been initialized, yet");
+    }
+    return { entry->value.cast<T>(), Timestamp{ entry->sequence_id, entry->stamp } };
+  }
+  throw RuntimeError("Blackboard::get() error. Missing key [", key, "]");
+}
+
 }  // namespace BT

include/behaviortree_cpp/tree_node.h

@@ -223,6 +223,9 @@ class TreeNode
   template <typename T>
   Result getInput(const std::string& key, T& destination) const;
 
+  template <typename T>
+  ResultStamped getInputStamped(const std::string& key, T& destination) const;
+
   /** Same as bool getInput(const std::string& key, T& destination)
    * but using optional.
    *
@@ -352,6 +355,9 @@ class TreeNode
   PreScripts& preConditionsScripts();
   PostScripts& postConditionsScripts();
 
+  template <typename T>
+  T parseString(const std::string& str) const;
+
 private:
   struct PImpl;
   std::unique_ptr<PImpl> _p;
@@ -365,32 +371,31 @@ class TreeNode
 };
 
 //-------------------------------------------------------
+
 template <typename T>
-inline Result TreeNode::getInput(const std::string& key, T& destination) const
+T TreeNode::parseString(const std::string& str) const
 {
-  // address the special case where T is an enum
-  auto ParseString = [this](const std::string& str) -> T {
-    (void)this;  // maybe unused
-    if constexpr(std::is_enum_v<T> && !std::is_same_v<T, NodeStatus>)
+  if constexpr(std::is_enum_v<T> && !std::is_same_v<T, NodeStatus>)
+  {
+    auto it = config().enums->find(str);
+    // conversion available
+    if(it != config().enums->end())
     {
-      auto it = config().enums->find(str);
-      // conversion available
-      if(it != config().enums->end())
-      {
-        return static_cast<T>(it->second);
-      }
-      else
-      {
-        // hopefully str contains a number that can be parsed. May throw
-        return static_cast<T>(convertFromString<int>(str));
-      }
+      return static_cast<T>(it->second);
     }
     else
     {
-      return convertFromString<T>(str);
+      // hopefully str contains a number that can be parsed. May throw
+      return static_cast<T>(convertFromString<int>(str));
     }
-  };
+  }
+  return convertFromString<T>(str);
+}
 
+template <typename T>
+inline ResultStamped TreeNode::getInputStamped(const std::string& key,
+                                               T& destination) const
+{
   std::string port_value_str;
 
   auto input_port_it = config().input_ports.find(key);
@@ -406,8 +411,7 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
     {
       return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(),
                                             "' failed because the manifest doesn't "
-                                            "contain"
-                                            "the key: [",
+                                            "contain the key: [",
                                             key, "]"));
     }
     const auto& port_info = port_manifest_it->second;
@@ -416,8 +420,7 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
     {
       return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(),
                                             "' failed because nor the manifest or the "
-                                            "XML contain"
-                                            "the key: [",
+                                            "XML contain the key: [",
                                             key, "]"));
     }
     if(port_info.defaultValue().isString())
@@ -427,68 +430,81 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
     else
     {
       destination = port_info.defaultValue().cast<T>();
-      return {};
+      return Timestamp{};
     }
   }
 
-  auto remapped_res = getRemappedKey(key, port_value_str);
+  auto blackboard_ptr = getRemappedKey(key, port_value_str);
   try
   {
     // pure string, not a blackboard key
-    if(!remapped_res)
+    if(!blackboard_ptr)
     {
       try
       {
-        destination = ParseString(port_value_str);
+        destination = parseString<T>(port_value_str);
       }
       catch(std::exception& ex)
       {
         return nonstd::make_unexpected(StrCat("getInput(): ", ex.what()));
       }
-      return {};
+      return Timestamp{};
     }
-    const auto& remapped_key = remapped_res.value();
+    const auto& blackboard_key = blackboard_ptr.value();
 
     if(!config().blackboard)
     {
       return nonstd::make_unexpected("getInput(): trying to access "
                                      "an invalid Blackboard");
     }
 
-    if(auto any_ref = config().blackboard->getAnyLocked(std::string(remapped_key)))
+    if(auto entry = config().blackboard->getEntry(std::string(blackboard_key)))
     {
-      auto val = any_ref.get();
+      std::unique_lock lk(entry->entry_mutex);
+      auto& any_value = entry->value;
+
       // support getInput<Any>()
       if constexpr(std::is_same_v<T, Any>)
       {
-        destination = *val;
+        destination = any_value;
         return {};
       }
 
-      if(!val->empty())
+      if(!entry->value.empty())
       {
-        if(!std::is_same_v<T, std::string> && val->isString())
+        if(!std::is_same_v<T, std::string> && any_value.isString())
         {
-          destination = ParseString(val->cast<std::string>());
+          destination = parseString<T>(any_value.cast<std::string>());
         }
         else
         {
-          destination = val->cast<T>();
+          destination = any_value.cast<T>();
         }
-        return {};
+        return Timestamp{ entry->sequence_id, entry->stamp };
       }
     }
 
     return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to "
                                           "find the key [",
-                                          key, "] remapped to [", remapped_key, "]"));
+                                          key, "] remapped to [", blackboard_key, "]"));
   }
   catch(std::exception& err)
   {
     return nonstd::make_unexpected(err.what());
   }
 }
 
+template <typename T>
+inline Result TreeNode::getInput(const std::string& key, T& destination) const
+{
+  auto res = getInputStamped(key, destination);
+  if(!res)
+  {
+    return nonstd::make_unexpected(res.error());
+  }
+  return {};
+}
+
 template <typename T>
 inline Result TreeNode::setOutput(const std::string& key, const T& value)
 {

src/blackboard.cpp

@@ -191,6 +191,8 @@ void Blackboard::cloneInto(Blackboard& dst) const
       dst_entry->string_converter = src_entry->string_converter;
       dst_entry->value = src_entry->value;
       dst_entry->info = src_entry->info;
+      dst_entry->sequence_id++;
+      dst_entry->stamp = std::chrono::steady_clock::now().time_since_epoch();
     }
     else
     {
@@ -297,4 +299,14 @@ void ImportBlackboardFromJSON(const nlohmann::json& json, Blackboard& blackboard
   }
 }
 
+Blackboard::Entry& Blackboard::Entry::operator=(const Entry& other)
+{
+  value = other.value;
+  info = other.info;
+  string_converter = other.string_converter;
+  sequence_id = other.sequence_id;
+  stamp = other.stamp;
+  return *this;
+}
+
 }  // namespace BT