Merge remote-tracking branch 'upstream/stamped_blackboard'

Author: GregoryLeMasurier

CMakeLists.txt

@@ -104,7 +104,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
@@ -149,7 +151,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})

include/behaviortree_cpp/basic_types.h

@@ -61,6 +61,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>>;
 
@@ -331,6 +333,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/behavior_tree.h

@@ -34,6 +34,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

@@ -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/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

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
 {