add Stamped blackboard (#805)

* improve blackboard exporting

* add timestamp and sequence to blackboard entries

* new decorators using the entry sequence, added

* updated interface with Expected

* Apply suggestions from code review

Author: facontidavide

CMakeLists.txt

@@ -103,7 +103,9 @@ list(APPEND BT_SOURCE
     src/decorators/repeat_node.cpp
     src/decorators/retry_node.cpp
     src/decorators/timeout_node.cpp
+    src/decorators/skip_unless_updated.cpp
     src/decorators/subtree_node.cpp
+    src/decorators/wait_update.cpp
 
     src/controls/if_then_else_node.cpp
     src/controls/fallback_node.cpp

include/behaviortree_cpp/basic_types.h

@@ -332,6 +332,14 @@ using Optional = nonstd::expected<T, std::string>;
  * */
 using Result = Expected<std::monostate>;
 
+struct Timestamp
+{
+  // Number being incremented every time a new value is written
+  uint64_t seq = 0;
+  // Last update time. Nanoseconds since epoch
+  std::chrono::nanoseconds time = std::chrono::nanoseconds(0);
+};
+
 [[nodiscard]] bool IsAllowedPortName(StringView str);
 
 class TypeInfo

include/behaviortree_cpp/behavior_tree.h

@@ -33,6 +33,8 @@
 #include "behaviortree_cpp/decorators/run_once_node.h"
 #include "behaviortree_cpp/decorators/subtree_node.h"
 #include "behaviortree_cpp/decorators/loop_node.h"
+#include "behaviortree_cpp/decorators/skip_unless_updated.h"
+#include "behaviortree_cpp/decorators/wait_update.h"
 
 #include "behaviortree_cpp/actions/always_success_node.h"
 #include "behaviortree_cpp/actions/always_failure_node.h"

include/behaviortree_cpp/blackboard.h

@@ -18,6 +18,13 @@ namespace BT
 /// with a locked mutex as long as the object is in scope
 using AnyPtrLocked = LockedPtr<Any>;
 
+template <typename T>
+struct StampedValue
+{
+  T value;
+  Timestamp stamp;
+};
+
 /**
  * @brief The Blackboard is the mechanism used by BehaviorTrees to exchange
  * typed data.
@@ -40,8 +47,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 +88,18 @@ class Blackboard
   template <typename T>
   [[nodiscard]] bool get(const std::string& key, T& value) const;
 
+  template <typename T>
+  [[nodiscard]] Expected<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>
+  [[nodiscard]] Expected<StampedValue<T>> 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 +174,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 +219,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 +230,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 +292,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 +302,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 Expected<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 nonstd::make_unexpected(StrCat("Blackboard::getStamped() error. Entry [",
+                                            key, "] hasn't been initialized, yet"));
+    }
+    value = entry->value.cast<T>();
+    return Timestamp{ entry->sequence_id, entry->stamp };
+  }
+  return nonstd::make_unexpected(
+      StrCat("Blackboard::getStamped() error. Missing key [", key, "]"));
+}
+
+template <typename T>
+inline Expected<StampedValue<T>> Blackboard::getStamped(const std::string& key) const
+{
+  StampedValue<T> out;
+  if(auto res = getStamped<T>(key, out.value))
+  {
+    out.stamp = *res;
+    return out;
+  }
+  else
+  {
+    return nonstd::make_unexpected(res.error());
+  }
+}
+
 }  // namespace BT

include/behaviortree_cpp/bt_factory.h

@@ -14,14 +14,11 @@
 #ifndef BT_FACTORY_H
 #define BT_FACTORY_H
 
-#include <algorithm>
-#include <cstring>
 #include <filesystem>
 #include <functional>
 #include <memory>
 #include <unordered_map>
 #include <set>
-#include <utility>
 #include <vector>
 
 #include "behaviortree_cpp/contrib/magic_enum.hpp"

include/behaviortree_cpp/decorators/skip_unless_updated.h

@@ -0,0 +1,50 @@
+/* Copyright (C) 2024 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#pragma once
+
+#include "behaviortree_cpp/decorator_node.h"
+
+namespace BT
+{
+
+/**
+ * @brief The SkipUnlessUpdated checks the Timestamp in an entry
+ * to determine if the value was updated since the last time (true,
+ * the first time).
+ *
+ * If it is, the child will be executed, otherwise SKIPPED is returned.
+ */
+class SkipUnlessUpdated : public DecoratorNode
+{
+public:
+  SkipUnlessUpdated(const std::string& name, const NodeConfig& config);
+
+  ~SkipUnlessUpdated() override = default;
+
+  static PortsList providedPorts()
+  {
+    return { InputPort<BT::Any>("entry", "Skip this branch unless the blackboard value "
+                                         "was updated") };
+  }
+
+private:
+  int64_t sequence_id_ = -1;
+  std::string entry_key_;
+  bool still_executing_child_ = false;
+
+  NodeStatus tick() override;
+
+  void halt() override;
+};
+
+}  // namespace BT

include/behaviortree_cpp/decorators/wait_update.h

@@ -0,0 +1,49 @@
+/* Copyright (C) 2024 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#pragma once
+
+#include "behaviortree_cpp/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The WaitValueUpdate checks the Timestamp in an entry
+ * to determine if the value was updated since the last time (true,
+ * the first time).
+ *
+ * If it is, the child will be executed, otherwise RUNNING is returned.
+ */
+class WaitValueUpdate : public DecoratorNode
+{
+public:
+  WaitValueUpdate(const std::string& name, const NodeConfig& config);
+
+  ~WaitValueUpdate() override = default;
+
+  static PortsList providedPorts()
+  {
+    return { InputPort<BT::Any>("entry", "Sleep until the entry in the blackboard is "
+                                         "updated") };
+  }
+
+private:
+  int64_t sequence_id_ = -1;
+  std::string entry_key_;
+  bool still_executing_child_ = false;
+
+  NodeStatus tick() override;
+
+  void halt() override;
+};
+
+}  // namespace BT