From 22f1b62a3a6d88b8feea63868b50522d7cff4360 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 12 Apr 2024 19:31:14 +0200 Subject: [PATCH] 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; * */ using Result = Expected; +struct Timestamp +{ + uint64_t seq = 0; + std::chrono::nanoseconds stamp = std::chrono::nanoseconds(0); +}; + +using ResultStamped = Expected; + [[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 #include #include +#include #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 [[nodiscard]] bool get(const std::string& key, T& value) const; + template + std::optional getStamped(const std::string& key, T& value) const; + /** * Version of get() that throws if it fails. */ template [[nodiscard]] T get(const std::string& key) const; + template + std::pair getStamped(const std::string& key) const; + /// Update the entry with the given key template 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(); } - 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(); + 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(); return true; } return false; } +template +inline std::optional 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(); + return Timestamp{ entry->sequence_id, entry->stamp }; + } + return std::nullopt; +} + +template +inline std::pair 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(), 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 Result getInput(const std::string& key, T& destination) const; + template + 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 + T parseString(const std::string& str) const; + private: struct PImpl; std::unique_ptr _p; @@ -365,32 +371,31 @@ class TreeNode }; //------------------------------------------------------- + template -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 && !std::is_same_v) + if constexpr(std::is_enum_v && !std::is_same_v) + { + 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(it->second); - } - else - { - // hopefully str contains a number that can be parsed. May throw - return static_cast(convertFromString(str)); - } + return static_cast(it->second); } else { - return convertFromString(str); + // hopefully str contains a number that can be parsed. May throw + return static_cast(convertFromString(str)); } - }; + } + return convertFromString(str); +} +template +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(); - 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(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() if constexpr(std::is_same_v) { - destination = *val; + destination = any_value; return {}; } - if(!val->empty()) + if(!entry->value.empty()) { - if(!std::is_same_v && val->isString()) + if(!std::is_same_v && any_value.isString()) { - destination = ParseString(val->cast()); + destination = parseString(any_value.cast()); } else { - destination = val->cast(); + destination = any_value.cast(); } - 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 +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 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