diff --git a/CMakeLists.txt b/CMakeLists.txt index 067d62bc6..e9a51414b 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 diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index ffa14b3c8..244f4b2a3 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 +{ + // 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 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/blackboard.h b/include/behaviortree_cpp/blackboard.h index 17bf9caef..5073fafb1 100644 --- a/include/behaviortree_cpp/blackboard.h +++ b/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; +template +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 [[nodiscard]] bool get(const std::string& key, T& value) const; + template + [[nodiscard]] Expected 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 + [[nodiscard]] Expected> 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 +174,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 +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,7 +230,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 +237,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 +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(); return true; } return false; } +template +inline Expected 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(); + return Timestamp{ entry->sequence_id, entry->stamp }; + } + return nonstd::make_unexpected( + StrCat("Blackboard::getStamped() error. Missing key [", key, "]")); +} + +template +inline Expected> Blackboard::getStamped(const std::string& key) const +{ + StampedValue out; + if(auto res = getStamped(key, out.value)) + { + out.stamp = *res; + return out; + } + else + { + return nonstd::make_unexpected(res.error()); + } +} + } // namespace BT 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 -#include #include #include #include #include #include -#include #include #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("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("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 3bd586a29..344b04427 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -218,11 +218,24 @@ class TreeNode * convertFromString() is used automatically to parse the text. * * @param key the name of the port. + * @param destination reference to the object where the value should be stored * @return false if an error occurs. */ template Result getInput(const std::string& key, T& destination) const; + /** + * @brief getInputStamped is similar to getInput(dey, destination), + * but it returne also the Timestamp object, that can be used to check if + * a value was updated and when. + * + * @param key the name of the port. + * @param destination reference to the object where the value should be stored + */ + template + [[nodiscard]] Expected getInputStamped(const std::string& key, + T& destination) const; + /** Same as bool getInput(const std::string& key, T& destination) * but using optional. * @@ -236,6 +249,26 @@ class TreeNode return (res) ? Expected(out) : nonstd::make_unexpected(res.error()); } + /** Same as bool getInputStamped(const std::string& key, T& destination) + * but return StampedValue + * + * @param key the name of the port. + */ + template + [[nodiscard]] Expected> getInputStamped(const std::string& key) const + { + StampedValue out; + if(auto res = getInputStamped(key, out.value)) + { + out.stamp = *res; + return out; + } + else + { + return nonstd::make_unexpected(res.error()); + } + } + /** * @brief setOutput modifies the content of an Output port * @param key the name of the port. @@ -352,6 +385,9 @@ class TreeNode PreScripts& preConditionsScripts(); PostScripts& postConditionsScripts(); + template + T parseString(const std::string& str) const; + private: struct PImpl; std::unique_ptr _p; @@ -365,32 +401,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 Expected 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 +441,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 +450,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 +460,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 +488,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; - return {}; + destination = any_value; + return Timestamp{ entry->sequence_id, entry->stamp }; } - 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 +524,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 b52d3d4ec..552efa00b 100644 --- a/src/blackboard.cpp +++ b/src/blackboard.cpp @@ -1,4 +1,5 @@ #include "behaviortree_cpp/blackboard.h" +#include #include "behaviortree_cpp/json_export.h" namespace BT @@ -166,15 +167,46 @@ 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 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; + dst_entry->sequence_id++; + dst_entry->stamp = std::chrono::steady_clock::now().time_since_epoch(); + } + else + { + // create new + auto new_entry = std::make_shared(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->info); - new_entry->value = entry->value; - new_entry->string_converter = entry->string_converter; - dst.storage_.insert({ key, new_entry }); + dst_storage.erase(key); } } @@ -267,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 diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 50719cb73..cbd9a59eb 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -95,6 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType>("LoopDouble"); registerNodeType>("LoopString"); + registerNodeType("SkipUnlessUpdated"); + registerNodeType("WaitValueUpdate"); + for(const auto& it : _p->builders) { _p->builtin_IDs.insert(it.first); @@ -697,25 +700,32 @@ std::vector BlackboardBackup(const Tree& tree) nlohmann::json ExportTreeToJSON(const Tree& tree) { - std::vector 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); } } diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/skip_unless_updated.cpp new file mode 100644 index 000000000..b0865fc3f --- /dev/null +++ b/src/decorators/skip_unless_updated.cpp @@ -0,0 +1,62 @@ +/* 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. +*/ + +#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(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(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 diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp index 9d9a7f950..c6caaa464 100644 --- a/tests/gtest_blackboard.cpp +++ b/tests/gtest_blackboard.cpp @@ -606,3 +606,42 @@ TEST(BlackboardTest, RootBlackboard) ASSERT_EQ(3, tree.rootBlackboard()->get("var3")); ASSERT_EQ(4, tree.rootBlackboard()->get("var4")); } + +TEST(BlackboardTest, TimestampedInterface) +{ + auto bb = BT::Blackboard::create(); + + // still empty, expected to fail + int value; + ASSERT_FALSE(bb->getStamped("value")); + ASSERT_FALSE(bb->getStamped("value", value)); + + auto nsec_before = std::chrono::steady_clock::now().time_since_epoch().count(); + bb->set("value", 42); + auto result = bb->getStamped("value"); + auto stamp_opt = bb->getStamped("value", value); + + ASSERT_EQ(result->value, 42); + ASSERT_EQ(result->stamp.seq, 1); + ASSERT_GE(result->stamp.time.count(), nsec_before); + + ASSERT_EQ(value, 42); + ASSERT_TRUE(stamp_opt); + ASSERT_EQ(stamp_opt->seq, 1); + ASSERT_GE(stamp_opt->time.count(), nsec_before); + + //--------------------------------- + nsec_before = std::chrono::steady_clock::now().time_since_epoch().count(); + bb->set("value", 69); + result = bb->getStamped("value"); + stamp_opt = bb->getStamped("value", value); + + ASSERT_EQ(result->value, 69); + ASSERT_EQ(result->stamp.seq, 2); + ASSERT_GE(result->stamp.time.count(), nsec_before); + + ASSERT_EQ(value, 69); + ASSERT_TRUE(stamp_opt); + ASSERT_EQ(stamp_opt->seq, 2); + ASSERT_GE(stamp_opt->time.count(), nsec_before); +}