From 1ef1c4d278c636c7183bf596356723cfd60876f0 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Fri, 5 Apr 2024 16:54:39 +0200
Subject: [PATCH 1/4] add syntax for entries in the root blackboard

---
 include/behaviortree_cpp/basic_types.h        |  2 +
 .../behaviortree_cpp/scripting/any_types.hpp  | 50 +++++++++++---
 .../behaviortree_cpp/scripting/operators.hpp  | 16 ++---
 src/basic_types.cpp                           |  5 ++
 src/blackboard.cpp                            | 66 +++++++++----------
 tests/gtest_blackboard.cpp                    | 44 +++++++++++++
 6 files changed, 128 insertions(+), 55 deletions(-)

diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h
index 18a3a931c..ffa14b3c8 100644
--- a/include/behaviortree_cpp/basic_types.h
+++ b/include/behaviortree_cpp/basic_types.h
@@ -60,6 +60,8 @@ using StringView = std::string_view;
 
 bool StartWith(StringView str, StringView prefix);
 
+bool StartWith(StringView str, char prefix);
+
 // vector of key/value pairs
 using KeyValueVector = std::vector<std::pair<std::string, std::string>>;
 
diff --git a/include/behaviortree_cpp/scripting/any_types.hpp b/include/behaviortree_cpp/scripting/any_types.hpp
index a757bec7f..0db106288 100644
--- a/include/behaviortree_cpp/scripting/any_types.hpp
+++ b/include/behaviortree_cpp/scripting/any_types.hpp
@@ -1,4 +1,4 @@
-/*  Copyright (C) 2022 Davide Faconti -  All Rights Reserved
+/*  Copyright (C) 2022-24 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,
@@ -12,14 +12,6 @@
 
 #pragma once
 
-#include <cmath>
-#include <cstdio>
-#include <map>
-#include <string>
-#include <vector>
-#include <utility>
-#include <charconv>
-
 #include "lexy/action/parse.hpp"
 #include "lexy/callback.hpp"
 #include "lexy/dsl.hpp"
@@ -31,6 +23,46 @@ namespace BT::Grammar
 {
 namespace dsl = lexy::dsl;
 
+struct _xid_start_character : lexyd::char_class_base<_xid_start_character>
+{
+  static LEXY_CONSTEVAL auto char_class_name()
+  {
+    return "code-point.BT-start-character";
+  }
+
+  static LEXY_CONSTEVAL auto char_class_ascii()
+  {
+    lexy::_detail::ascii_set result;
+    result.insert('a', 'z');
+    result.insert('A', 'Z');
+    result.insert('_');
+    result.insert('@');
+    return result;
+  }
+
+  static LEXY_UNICODE_CONSTEXPR bool char_class_match_cp(char32_t cp)
+  {
+    // underscore handled as part of ASCII.
+    return lexy::_detail::code_point_has_properties<LEXY_UNICODE_PROPERTY(xid_start)>(cp);
+  }
+
+  template <typename Encoding>
+  static constexpr auto char_class_match_swar(lexy::_detail::swar_int c)
+  {
+    return lexyd::ascii::_alphau::template char_class_match_swar<Encoding>(c);
+  }
+};
+inline constexpr auto xid_start_character = _xid_start_character{};
+
+// A Unicode-aware identifier.
+struct Name
+{
+  static constexpr auto rule =
+      dsl::identifier(xid_start_character, dsl::unicode::xid_continue);
+
+  static constexpr auto value = lexy::as_string<std::string>;
+};
+
 //----------
 struct Integer : lexy::token_production
 {
diff --git a/include/behaviortree_cpp/scripting/operators.hpp b/include/behaviortree_cpp/scripting/operators.hpp
index b38132d17..176fa2df0 100644
--- a/include/behaviortree_cpp/scripting/operators.hpp
+++ b/include/behaviortree_cpp/scripting/operators.hpp
@@ -510,6 +510,10 @@ struct ExprAssignment : ExprBase
       {
         env.vars->createEntry(key, PortInfo());
         entry = env.vars->getEntry(key);
+        if(!entry)
+        {
+          throw LogicError("Bug: report");
+        }
       }
       else
       {
@@ -550,7 +554,8 @@ struct ExprAssignment : ExprBase
         // special case: string to other type.
         // Check if we can use the StringConverter
         auto const str = value.cast<std::string>();
-        const auto& entry_info = env.vars->entryInfo(key);
+        const auto* entry_info = env.vars->entryInfo(key);
+
         if(auto converter = entry_info->converter())
         {
           *dst_ptr = converter(str);
@@ -648,15 +653,6 @@ namespace dsl = lexy::dsl;
 
 constexpr auto escaped_newline = dsl::backslash >> dsl::newline;
 
-// A Unicode-aware identifier.
-struct Name
-{
-  static constexpr auto rule =
-      dsl::identifier(dsl::unicode::xid_start_underscore, dsl::unicode::xid_continue);
-
-  static constexpr auto value = lexy::as_string<std::string>;
-};
-
 // An expression that is nested inside another expression.
 struct nested_expr : lexy::transparent_production
 {
diff --git a/src/basic_types.cpp b/src/basic_types.cpp
index 8265365b8..ad7f7199c 100644
--- a/src/basic_types.cpp
+++ b/src/basic_types.cpp
@@ -467,4 +467,9 @@ bool StartWith(StringView str, StringView prefix)
          strncmp(str.data(), prefix.data(), prefix.size()) == 0;
 }
 
+bool StartWith(StringView str, char prefix)
+{
+  return str.size() >= 1 && str[0] == prefix;
+}
+
 }  // namespace BT
diff --git a/src/blackboard.cpp b/src/blackboard.cpp
index eecc409d6..b52d3d4ec 100644
--- a/src/blackboard.cpp
+++ b/src/blackboard.cpp
@@ -45,38 +45,25 @@ Any* Blackboard::getAny(const std::string& key)
 const std::shared_ptr<Blackboard::Entry>
 Blackboard::getEntry(const std::string& key) const
 {
-  std::unique_lock<std::mutex> lock(mutex_);
-  auto it = storage_.find(key);
-  if(it != storage_.end())
+  // special syntax: "@" will always refer to the root BB
+  if(StartWith(key, '@'))
   {
-    return it->second;
-  }
-  // not found. Try autoremapping
-  if(auto parent = parent_bb_.lock())
-  {
-    auto remap_it = internal_to_external_.find(key);
-    if(remap_it != internal_to_external_.cend())
+    if(auto parent = parent_bb_.lock())
     {
-      auto const& new_key = remap_it->second;
-      return parent->getEntry(new_key);
+      return parent->getEntry(key);
     }
-    if(autoremapping_ && !IsPrivateKey(key))
+    else
     {
-      return parent->getEntry(key);
+      return getEntry(key.substr(1, key.size() - 1));
     }
   }
-  return {};
-}
 
-std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string& key)
-{
   std::unique_lock<std::mutex> lock(mutex_);
   auto it = storage_.find(key);
   if(it != storage_.end())
   {
     return it->second;
   }
-
   // not found. Try autoremapping
   if(auto parent = parent_bb_.lock())
   {
@@ -84,32 +71,25 @@ std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string& key)
     if(remap_it != internal_to_external_.cend())
     {
       auto const& new_key = remap_it->second;
-      auto entry = parent->getEntry(new_key);
-      if(entry)
-      {
-        storage_.insert({ key, entry });
-      }
-      return entry;
+      return parent->getEntry(new_key);
     }
     if(autoremapping_ && !IsPrivateKey(key))
     {
-      auto entry = parent->getEntry(key);
-      if(entry)
-      {
-        storage_.insert({ key, entry });
-      }
-      return entry;
+      return parent->getEntry(key);
     }
   }
   return {};
 }
 
-const TypeInfo* Blackboard::entryInfo(const std::string& key)
+std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string& key)
 {
-  std::unique_lock<std::mutex> lock(mutex_);
+  return static_cast<const Blackboard&>(*this).getEntry(key);
+}
 
-  auto it = storage_.find(key);
-  return (it == storage_.end()) ? nullptr : &(it->second->info);
+const TypeInfo* Blackboard::entryInfo(const std::string& key)
+{
+  auto entry = getEntry(key);
+  return (!entry) ? nullptr : &(entry->info);
 }
 
 void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
@@ -167,7 +147,21 @@ std::recursive_mutex& Blackboard::entryMutex() const
 
 void Blackboard::createEntry(const std::string& key, const TypeInfo& info)
 {
-  createEntryImpl(key, info);
+  if(StartWith(key, '@'))
+  {
+    if(auto parent = parent_bb_.lock())
+    {
+      parent->createEntry(key, info);
+    }
+    else
+    {
+      createEntryImpl(key.substr(1, key.size() - 1), info);
+    }
+  }
+  else
+  {
+    createEntryImpl(key, info);
+  }
 }
 
 void Blackboard::cloneInto(Blackboard& dst) const
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index 980db1e90..98eaa8c99 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -557,3 +557,47 @@ TEST(BlackboardTest, BlackboardBackup)
   status = tree.tickWhileRunning();
   ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
 }
+
+TEST(BlackboardTest, RootBlackboard)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+  <root BTCPP_format="4" >
+    <BehaviorTree ID="SubA">
+      <Sequence>
+        <SubTree ID="SubB" />
+        <Script code=" @var3:=3 " />
+      </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="SubB">
+      <Sequence>
+        <SaySomething message="{@msg}" />
+        <Script code=" @var4:=4 " />
+      </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="MainTree">
+      <Sequence>
+        <Script code=" msg:='hello' " />
+        <SubTree ID="SubA" />
+
+        <Script code=" var1:=1 " />
+        <Script code=" @var2:=2 " />
+      </Sequence>
+    </BehaviorTree>
+  </root> )";
+
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+
+  auto status = tree.tickWhileRunning();
+  ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
+
+  ASSERT_EQ(1, tree.rootBlackboard()->get<int>("var1"));
+  ASSERT_EQ(2, tree.rootBlackboard()->get<int>("var2"));
+  ASSERT_EQ(3, tree.rootBlackboard()->get<int>("var3"));
+  ASSERT_EQ(4, tree.rootBlackboard()->get<int>("var4"));
+}

From 720cf63b7d0599ebb182e4b4ec6a3d48f717ff5d Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Fri, 12 Apr 2024 18:42:08 +0200
Subject: [PATCH 2/4] improve blackboard exporting

---
 src/blackboard.cpp | 44 +++++++++++++++++++++++++++++++++++++-------
 src/bt_factory.cpp | 21 ++++++++++++++-------
 2 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/src/blackboard.cpp b/src/blackboard.cpp
index b52d3d4ec..d32414092 100644
--- a/src/blackboard.cpp
+++ b/src/blackboard.cpp
@@ -1,4 +1,5 @@
 #include "behaviortree_cpp/blackboard.h"
+#include <unordered_set>
 #include "behaviortree_cpp/json_export.h"
 
 namespace BT
@@ -166,15 +167,44 @@ void Blackboard::createEntry(const std::string& key, const TypeInfo& info)
 
 void Blackboard::cloneInto(Blackboard& dst) const
 {
-  std::unique_lock lk(dst.mutex_);
-  dst.storage_.clear();
+  std::unique_lock lk1(mutex_);
+  std::unique_lock lk2(dst.mutex_);
 
-  for(const auto& [key, entry] : storage_)
+  // keys that are not updated must be removed.
+  std::unordered_set<std::string> keys_to_remove;
+  auto& dst_storage = dst.storage_;
+  for(const auto& [key, _] : dst_storage)
+  {
+    keys_to_remove.insert(key);
+  }
+
+  // update or create entries in dst_storage
+  for(const auto& [src_key, src_entry] : storage_)
+  {
+    keys_to_remove.erase(src_key);
+
+    auto it = dst_storage.find(src_key);
+    if(it != dst_storage.end())
+    {
+      // overwite
+      auto& dst_entry = it->second;
+      dst_entry->string_converter = src_entry->string_converter;
+      dst_entry->value = src_entry->value;
+      dst_entry->info = src_entry->info;
+    }
+    else
+    {
+      // create new
+      auto new_entry = std::make_shared<Entry>(src_entry->info);
+      new_entry->value = src_entry->value;
+      new_entry->string_converter = src_entry->string_converter;
+      dst_storage.insert({ src_key, new_entry });
+    }
+  }
+
+  for(const auto& key : keys_to_remove)
   {
-    auto new_entry = std::make_shared<Entry>(entry->info);
-    new_entry->value = entry->value;
-    new_entry->string_converter = entry->string_converter;
-    dst.storage_.insert({ key, new_entry });
+    dst_storage.erase(key);
   }
 }
 
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
index 50719cb73..beb6c8b5d 100644
--- a/src/bt_factory.cpp
+++ b/src/bt_factory.cpp
@@ -697,25 +697,32 @@ std::vector<Blackboard::Ptr> BlackboardBackup(const Tree& tree)
 
 nlohmann::json ExportTreeToJSON(const Tree& tree)
 {
-  std::vector<nlohmann::json> bbs;
+  nlohmann::json out;
   for(const auto& subtree : tree.subtrees)
   {
-    bbs.push_back(ExportBlackboardToJSON(*subtree->blackboard));
+    nlohmann::json json_sub;
+    auto sub_name = subtree->instance_name;
+    if(sub_name.empty())
+    {
+      sub_name = subtree->tree_ID;
+    }
+    out[sub_name] = ExportBlackboardToJSON(*subtree->blackboard);
   }
-  return bbs;
+  return out;
 }
 
 void ImportTreeFromJSON(const nlohmann::json& json, Tree& tree)
 {
   if(json.size() != tree.subtrees.size())
   {
-    std::cerr << "Number of blackboards don't match:" << json.size() << "/"
-              << tree.subtrees.size() << "\n";
     throw std::runtime_error("Number of blackboards don't match:");
   }
-  for(size_t i = 0; i < tree.subtrees.size(); i++)
+
+  size_t index = 0;
+  for(auto& [key, array] : json.items())
   {
-    ImportBlackboardFromJSON(json.at(i), *tree.subtrees.at(i)->blackboard);
+    auto& subtree = tree.subtrees.at(index++);
+    ImportBlackboardFromJSON(array, *subtree->blackboard);
   }
 }
 

From 22f1b62a3a6d88b8feea63868b50522d7cff4360 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Fri, 12 Apr 2024 19:31:14 +0200
Subject: [PATCH 3/4] add timestamp and sequence to blackboard entries

---
 include/behaviortree_cpp/basic_types.h |  8 +++
 include/behaviortree_cpp/blackboard.h  | 62 ++++++++++++++++--
 include/behaviortree_cpp/tree_node.h   | 90 +++++++++++++++-----------
 src/blackboard.cpp                     | 12 ++++
 4 files changed, 130 insertions(+), 42 deletions(-)

diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h
index ffa14b3c8..04fe878dc 100644
--- a/include/behaviortree_cpp/basic_types.h
+++ b/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
diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h
index 17bf9caef..3be6d3444 100644
--- a/include/behaviortree_cpp/blackboard.h
+++ b/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,7 +224,6 @@ 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
@@ -220,6 +231,8 @@ inline void Blackboard::set(const std::string& key, const T& value)
     {
       // 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
diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h
index 3bd586a29..ef2ba9331 100644
--- a/include/behaviortree_cpp/tree_node.h
+++ b/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,27 +430,27 @@ 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)
     {
@@ -455,33 +458,35 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
                                      "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)
   {
@@ -489,6 +494,17 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
   }
 }
 
+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)
 {
diff --git a/src/blackboard.cpp b/src/blackboard.cpp
index d32414092..552efa00b 100644
--- a/src/blackboard.cpp
+++ b/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

From 3a9784f9703733b7e69190852fce5000ad3bccb1 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Sat, 13 Apr 2024 11:26:48 +0200
Subject: [PATCH 4/4] new decorators using the entry sequence, added

---
 CMakeLists.txt                                |  5 +-
 include/behaviortree_cpp/behavior_tree.h      |  2 +
 include/behaviortree_cpp/bt_factory.h         |  3 -
 .../decorators/skip_unless_updated.h          | 50 +++++++++++++++
 .../behaviortree_cpp/decorators/wait_update.h | 49 ++++++++++++++
 include/behaviortree_cpp/tree_node.h          |  2 +-
 src/bt_factory.cpp                            |  3 +
 src/decorators/skip_unless_updated.cpp        | 64 +++++++++++++++++++
 src/decorators/wait_update.cpp                | 64 +++++++++++++++++++
 9 files changed, 237 insertions(+), 5 deletions(-)
 create mode 100644 include/behaviortree_cpp/decorators/skip_unless_updated.h
 create mode 100644 include/behaviortree_cpp/decorators/wait_update.h
 create mode 100644 src/decorators/skip_unless_updated.cpp
 create mode 100644 src/decorators/wait_update.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 067d62bc6..68506e0e8 100644
--- a/CMakeLists.txt
+++ b/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
@@ -148,7 +150,8 @@ endif()
 
 if (BTCPP_SHARED_LIBS)
     set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
-    add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE})
+    add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE}
+        src/decorators/wait_update.cpp)
 else()
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
     add_library(${BTCPP_LIBRARY} STATIC ${BT_SOURCE})
diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h
index e26cf53e2..346303304 100644
--- a/include/behaviortree_cpp/behavior_tree.h
+++ b/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"
diff --git a/include/behaviortree_cpp/bt_factory.h b/include/behaviortree_cpp/bt_factory.h
index 05ecf9b9d..99b27d4ba 100644
--- a/include/behaviortree_cpp/bt_factory.h
+++ b/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"
diff --git a/include/behaviortree_cpp/decorators/skip_unless_updated.h b/include/behaviortree_cpp/decorators/skip_unless_updated.h
new file mode 100644
index 000000000..62e86aa9f
--- /dev/null
+++ b/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
diff --git a/include/behaviortree_cpp/decorators/wait_update.h b/include/behaviortree_cpp/decorators/wait_update.h
new file mode 100644
index 000000000..d4d4487d9
--- /dev/null
+++ b/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
diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h
index ef2ba9331..ca38916e0 100644
--- a/include/behaviortree_cpp/tree_node.h
+++ b/include/behaviortree_cpp/tree_node.h
@@ -467,7 +467,7 @@ inline ResultStamped TreeNode::getInputStamped(const std::string& key,
       if constexpr(std::is_same_v<T, Any>)
       {
         destination = any_value;
-        return {};
+        return Timestamp{ entry->sequence_id, entry->stamp };
       }
 
       if(!entry->value.empty())
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
index beb6c8b5d..cbd9a59eb 100644
--- a/src/bt_factory.cpp
+++ b/src/bt_factory.cpp
@@ -95,6 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<LoopNode<double>>("LoopDouble");
   registerNodeType<LoopNode<std::string>>("LoopString");
 
+  registerNodeType<SkipUnlessUpdated>("SkipUnlessUpdated");
+  registerNodeType<WaitValueUpdate>("WaitValueUpdate");
+
   for(const auto& it : _p->builders)
   {
     _p->builtin_IDs.insert(it.first);
diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/skip_unless_updated.cpp
new file mode 100644
index 000000000..619052bc5
--- /dev/null
+++ b/src/decorators/skip_unless_updated.cpp
@@ -0,0 +1,64 @@
+/* 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/decorators/skip_unless_updated.h"
+
+namespace BT
+{
+
+SkipUnlessUpdated::SkipUnlessUpdated(const std::string& name, const NodeConfig& config)
+  : DecoratorNode(name, config)
+{
+  const auto entry_str = config.input_ports.at("entry");
+  StringView stripped_key;
+  if(isBlackboardPointer(entry_str, &stripped_key))
+  {
+    entry_key_ = stripped_key;
+  }
+  else
+  {
+    entry_key_ = entry_str;
+  }
+}
+
+NodeStatus SkipUnlessUpdated::tick()
+{
+  // continue executing an asynchronous child
+  if(still_executing_child_)
+  {
+    auto status = child()->executeTick();
+    still_executing_child_ = (status == NodeStatus::RUNNING);
+    return status;
+  }
+
+  auto entry = config().blackboard->getEntry(entry_key_);
+  std::unique_lock lk(entry->entry_mutex);
+  auto seq = static_cast<int64_t>(entry->sequence_id);
+  if(seq == sequence_id_)
+  {
+    return NodeStatus::SKIPPED;
+  }
+  sequence_id_ = seq;
+
+  auto status = child()->executeTick();
+  still_executing_child_ = (status == NodeStatus::RUNNING);
+  return status;
+}
+
+void SkipUnlessUpdated::halt()
+{
+  still_executing_child_ = false;
+}
+
+}  // namespace BT
diff --git a/src/decorators/wait_update.cpp b/src/decorators/wait_update.cpp
new file mode 100644
index 000000000..daf672cae
--- /dev/null
+++ b/src/decorators/wait_update.cpp
@@ -0,0 +1,64 @@
+/* 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/decorators/wait_update.h"
+
+namespace BT
+{
+
+WaitValueUpdate::WaitValueUpdate(const std::string& name, const NodeConfig& config)
+  : DecoratorNode(name, config)
+{
+  const auto entry_str = config.input_ports.at("entry");
+  StringView stripped_key;
+  if(isBlackboardPointer(entry_str, &stripped_key))
+  {
+    entry_key_ = stripped_key;
+  }
+  else
+  {
+    entry_key_ = entry_str;
+  }
+}
+
+NodeStatus WaitValueUpdate::tick()
+{
+  // continue executing an asynchronous child
+  if(still_executing_child_)
+  {
+    auto status = child()->executeTick();
+    still_executing_child_ = (status == NodeStatus::RUNNING);
+    return status;
+  }
+
+  auto entry = config().blackboard->getEntry(entry_key_);
+  std::unique_lock lk(entry->entry_mutex);
+  auto seq = static_cast<int64_t>(entry->sequence_id);
+  if(seq == sequence_id_)
+  {
+    return NodeStatus::RUNNING;
+  }
+  sequence_id_ = seq;
+
+  auto status = child()->executeTick();
+  still_executing_child_ = (status == NodeStatus::RUNNING);
+  return status;
+}
+
+void WaitValueUpdate::halt()
+{
+  still_executing_child_ = false;
+}
+
+}  // namespace BT