From e066971798aaefc034a2f9f3137de701b327f241 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 18 Apr 2024 16:53:09 +0200 Subject: [PATCH 001/106] 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 --- CMakeLists.txt | 2 + include/behaviortree_cpp/basic_types.h | 8 ++ include/behaviortree_cpp/behavior_tree.h | 2 + include/behaviortree_cpp/blackboard.h | 68 +++++++++- 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 | 122 ++++++++++++------ src/blackboard.cpp | 56 +++++++- src/bt_factory.cpp | 24 +++- src/decorators/skip_unless_updated.cpp | 62 +++++++++ src/decorators/wait_update.cpp | 64 +++++++++ tests/gtest_blackboard.cpp | 39 ++++++ 13 files changed, 489 insertions(+), 60 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..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); +} From 133c5fe449c444ad09c646179808d7f9d3a89ec1 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 22 Apr 2024 15:32:49 +0200 Subject: [PATCH 002/106] fix warning --- src/decorators/wait_update.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/decorators/wait_update.cpp b/src/decorators/wait_update.cpp index daf672cae..d9f5fde07 100644 --- a/src/decorators/wait_update.cpp +++ b/src/decorators/wait_update.cpp @@ -10,8 +10,6 @@ * 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 From 696a27be417ab2046abb39b6c375274cf619a5d1 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 22 Apr 2024 15:46:17 +0200 Subject: [PATCH 003/106] cleanup t12 --- examples/t12_groot_howto.cpp | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/examples/t12_groot_howto.cpp b/examples/t12_groot_howto.cpp index a682a63a9..4b3d824e8 100644 --- a/examples/t12_groot_howto.cpp +++ b/examples/t12_groot_howto.cpp @@ -2,7 +2,6 @@ #include "crossdoor_nodes.h" #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/loggers/groot2_publisher.h" -#include "behaviortree_cpp/loggers/bt_sqlite_logger.h" #include "behaviortree_cpp/xml_parsing.h" #include "behaviortree_cpp/json_export.h" @@ -119,26 +118,8 @@ int main() // Add two more loggers, to save the transitions into a file. // Both formats are compatible with Groot2 - // Lightweight serialization + // Logging with lightweight serialization BT::FileLogger2 logger2(tree, "t12_logger2.btlog"); - // SQLite logger can save multiple sessions into the same database - bool append_to_database = true; - BT::SqliteLogger sqlite_logger(tree, "t12_sqlitelog.db3", append_to_database); - - // We can add some extra information to the SqliteLogger, for instance the value of the - // "door_open" blackboard entry, at the end of node "tryOpen" (Fallback) - - auto sqlite_callback = [](BT::Duration timestamp, const BT::TreeNode& node, - BT::NodeStatus prev_status, - BT::NodeStatus status) -> std::string { - if(node.name() == "tryOpen" && BT::isStatusCompleted(status)) - { - auto is_open = BT::toStr(node.config().blackboard->get("door_open")); - return "[tryOpen] door_open=" + is_open; - } - return {}; - }; - sqlite_logger.setAdditionalCallback(sqlite_callback); while(1) { From 8abc277c64d90ef1ed6ae87da1b7c6ce6d57fbbf Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 11:40:09 +0200 Subject: [PATCH 004/106] fix deadlock --- src/decorators/skip_unless_updated.cpp | 14 ++++++++------ src/decorators/wait_update.cpp | 14 ++++++++------ 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/skip_unless_updated.cpp index b0865fc3f..53122ee17 100644 --- a/src/decorators/skip_unless_updated.cpp +++ b/src/decorators/skip_unless_updated.cpp @@ -40,14 +40,16 @@ NodeStatus SkipUnlessUpdated::tick() 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; + 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; } - sequence_id_ = seq; auto status = child()->executeTick(); still_executing_child_ = (status == NodeStatus::RUNNING); diff --git a/src/decorators/wait_update.cpp b/src/decorators/wait_update.cpp index d9f5fde07..6a56c46c4 100644 --- a/src/decorators/wait_update.cpp +++ b/src/decorators/wait_update.cpp @@ -40,14 +40,16 @@ NodeStatus WaitValueUpdate::tick() 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; + 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; } - sequence_id_ = seq; auto status = child()->executeTick(); still_executing_child_ = (status == NodeStatus::RUNNING); From fc041fec0fd2abac83efcfafa10903f60826f087 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 11:51:42 +0200 Subject: [PATCH 005/106] remove code duplication and fix mutex deadlock --- CMakeLists.txt | 5 +- include/behaviortree_cpp/behavior_tree.h | 3 +- ...unless_updated.h => entry_updated_nodes.h} | 16 ++--- .../behaviortree_cpp/decorators/wait_update.h | 49 -------------- src/bt_factory.cpp | 4 +- ...ss_updated.cpp => entry_updated_nodes.cpp} | 13 ++-- src/decorators/wait_update.cpp | 64 ------------------- 7 files changed, 20 insertions(+), 134 deletions(-) rename include/behaviortree_cpp/decorators/{skip_unless_updated.h => entry_updated_nodes.h} (77%) delete mode 100644 include/behaviortree_cpp/decorators/wait_update.h rename src/decorators/{skip_unless_updated.cpp => entry_updated_nodes.cpp} (84%) delete mode 100644 src/decorators/wait_update.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e9a51414b..c42689653 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,13 +99,12 @@ list(APPEND BT_SOURCE src/actions/sleep_node.cpp src/decorators/delay_node.cpp + src/decorators/entry_updated_nodes.cpp src/decorators/inverter_node.cpp 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/decorators/timeout_node.cpp src/controls/if_then_else_node.cpp src/controls/fallback_node.cpp diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index 346303304..008219cb7 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -33,8 +33,7 @@ #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/decorators/entry_updated_nodes.h" #include "behaviortree_cpp/actions/always_success_node.h" #include "behaviortree_cpp/actions/always_failure_node.h" diff --git a/include/behaviortree_cpp/decorators/skip_unless_updated.h b/include/behaviortree_cpp/decorators/entry_updated_nodes.h similarity index 77% rename from include/behaviortree_cpp/decorators/skip_unless_updated.h rename to include/behaviortree_cpp/decorators/entry_updated_nodes.h index 62e86aa9f..d9d20c4b0 100644 --- a/include/behaviortree_cpp/decorators/skip_unless_updated.h +++ b/include/behaviortree_cpp/decorators/entry_updated_nodes.h @@ -16,31 +16,31 @@ namespace BT { - /** - * @brief The SkipUnlessUpdated checks the Timestamp in an entry + * @brief The EntryUpdatedNode 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. + * If it is, the child will be executed, otherwise [if_not_updated] value is returned. */ -class SkipUnlessUpdated : public DecoratorNode +class EntryUpdatedNode : public DecoratorNode { public: - SkipUnlessUpdated(const std::string& name, const NodeConfig& config); + EntryUpdatedNode(const std::string& name, const NodeConfig& config, + NodeStatus if_not_updated); - ~SkipUnlessUpdated() override = default; + ~EntryUpdatedNode() override = default; static PortsList providedPorts() { - return { InputPort("entry", "Skip this branch unless the blackboard value " - "was updated") }; + return { InputPort("entry", "Entry to check") }; } private: int64_t sequence_id_ = -1; std::string entry_key_; bool still_executing_child_ = false; + NodeStatus if_not_updated_; NodeStatus tick() override; diff --git a/include/behaviortree_cpp/decorators/wait_update.h b/include/behaviortree_cpp/decorators/wait_update.h deleted file mode 100644 index d4d4487d9..000000000 --- a/include/behaviortree_cpp/decorators/wait_update.h +++ /dev/null @@ -1,49 +0,0 @@ -/* 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/src/bt_factory.cpp b/src/bt_factory.cpp index cbd9a59eb..964954773 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -95,8 +95,8 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType>("LoopDouble"); registerNodeType>("LoopString"); - registerNodeType("SkipUnlessUpdated"); - registerNodeType("WaitValueUpdate"); + registerNodeType("SkipUnlessUpdated", NodeStatus::SKIPPED); + registerNodeType("WaitValueUpdate", NodeStatus::RUNNING); for(const auto& it : _p->builders) { diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/entry_updated_nodes.cpp similarity index 84% rename from src/decorators/skip_unless_updated.cpp rename to src/decorators/entry_updated_nodes.cpp index 53122ee17..20dfd520d 100644 --- a/src/decorators/skip_unless_updated.cpp +++ b/src/decorators/entry_updated_nodes.cpp @@ -10,13 +10,14 @@ * 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" +#include "behaviortree_cpp/decorators/entry_updated_nodes.h" namespace BT { -SkipUnlessUpdated::SkipUnlessUpdated(const std::string& name, const NodeConfig& config) - : DecoratorNode(name, config) +EntryUpdatedNode::EntryUpdatedNode(const std::string& name, const NodeConfig& config, + NodeStatus if_not_updated) + : DecoratorNode(name, config), if_not_updated_(if_not_updated) { const auto entry_str = config.input_ports.at("entry"); StringView stripped_key; @@ -30,7 +31,7 @@ SkipUnlessUpdated::SkipUnlessUpdated(const std::string& name, const NodeConfig& } } -NodeStatus SkipUnlessUpdated::tick() +NodeStatus EntryUpdatedNode::tick() { // continue executing an asynchronous child if(still_executing_child_) @@ -46,7 +47,7 @@ NodeStatus SkipUnlessUpdated::tick() auto seq = static_cast(entry->sequence_id); if(seq == sequence_id_) { - return NodeStatus::SKIPPED; + return if_not_updated_; } sequence_id_ = seq; } @@ -56,7 +57,7 @@ NodeStatus SkipUnlessUpdated::tick() return status; } -void SkipUnlessUpdated::halt() +void EntryUpdatedNode::halt() { still_executing_child_ = false; } diff --git a/src/decorators/wait_update.cpp b/src/decorators/wait_update.cpp deleted file mode 100644 index 6a56c46c4..000000000 --- a/src/decorators/wait_update.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* 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/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::SKIPPED; - } - sequence_id_ = seq; - } - - auto status = child()->executeTick(); - still_executing_child_ = (status == NodeStatus::RUNNING); - return status; -} - -void WaitValueUpdate::halt() -{ - still_executing_child_ = false; -} - -} // namespace BT From db89b3d468e3a3cbfe806e4de5fdb15f2d089da4 Mon Sep 17 00:00:00 2001 From: "avikus-seonghyeon.kwon" Date: Wed, 24 Apr 2024 18:53:25 +0900 Subject: [PATCH 006/106] Add library alias for BT::behaviortree_cpp (#808) --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c42689653..1481cf006 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,6 +186,8 @@ else() target_compile_options(${BTCPP_LIBRARY} PRIVATE -Wall -Wextra) endif() +add_library(BT::${BTCPP_LIBRARY} ALIAS ${BTCPP_LIBRARY}) + ############################################################# message( STATUS "BTCPP_LIB_DESTINATION: ${BTCPP_LIB_DESTINATION} " ) message( STATUS "BTCPP_INCLUDE_DESTINATION: ${BTCPP_INCLUDE_DESTINATION} " ) From 0deb546a2575330918137ed79894f94c8301bf8f Mon Sep 17 00:00:00 2001 From: Sean Geles <58413041+seanng-1@users.noreply.github.com> Date: Wed, 24 Apr 2024 19:53:58 +1000 Subject: [PATCH 007/106] Add string concatenation operator to scripting (#802) * Add string concatenation operator to scripting * Linting --- .../behaviortree_cpp/scripting/operators.hpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/include/behaviortree_cpp/scripting/operators.hpp b/include/behaviortree_cpp/scripting/operators.hpp index 176fa2df0..14cf04f43 100644 --- a/include/behaviortree_cpp/scripting/operators.hpp +++ b/include/behaviortree_cpp/scripting/operators.hpp @@ -150,6 +150,7 @@ struct ExprBinaryArithmetic : ExprBase minus, times, div, + concat, bit_and, bit_or, @@ -171,6 +172,8 @@ struct ExprBinaryArithmetic : ExprBase return "*"; case div: return "/"; + case concat: + return ".."; case bit_and: return "&"; case bit_or: @@ -276,6 +279,12 @@ struct ExprBinaryArithmetic : ExprBase { return Any(lhs_v.cast() + rhs_v.cast()); } + else if(op == concat && ((rhs_v.isString() && lhs_v.isString()) || + (rhs_v.isString() && lhs_v.isNumber()) || + (rhs_v.isNumber() && lhs_v.isString()))) + { + return Any(lhs_v.cast() + rhs_v.cast()); + } else { throw RuntimeError("Operation not permitted"); @@ -722,6 +731,16 @@ struct Expression : lexy::expression_production using operand = math_product; }; + // x .. y + struct string_concat : dsl::infix_op_left + { + static constexpr auto op = [] { + return dsl::op(LEXY_LIT("..")); + }(); + + using operand = math_sum; + }; + // ~x struct bit_prefix : dsl::prefix_op { @@ -785,7 +804,7 @@ struct Expression : lexy::expression_production dsl::op(LEXY_LIT("||")) / dsl::op(LEXY_LIT("&&")); - using operand = comparison; + using operand = dsl::groups; }; // x ? y : z From ecd41b1c87cb4fb19de6d60f9c0a1219aad2e684 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 14:02:46 +0200 Subject: [PATCH 008/106] bug fixes related to sequence_id and unit tests added --- CMakeLists.txt | 3 +- .../behaviortree_cpp/actions/updated_action.h | 45 ++++++ include/behaviortree_cpp/behavior_tree.h | 3 +- ...ry_updated_nodes.h => updated_decorator.h} | 12 +- .../behaviortree_cpp/scripting/operators.hpp | 6 +- src/actions/updated_action.cpp | 69 ++++++++++ src/bt_factory.cpp | 5 +- ...pdated_nodes.cpp => updated_decorator.cpp} | 33 +++-- tests/CMakeLists.txt | 1 + tests/gtest_updates.cpp | 129 ++++++++++++++++++ 10 files changed, 285 insertions(+), 21 deletions(-) create mode 100644 include/behaviortree_cpp/actions/updated_action.h rename include/behaviortree_cpp/decorators/{entry_updated_nodes.h => updated_decorator.h} (83%) create mode 100644 src/actions/updated_action.cpp rename src/decorators/{entry_updated_nodes.cpp => updated_decorator.cpp} (68%) create mode 100644 tests/gtest_updates.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c42689653..3747119b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -97,14 +97,15 @@ list(APPEND BT_SOURCE src/actions/test_node.cpp src/actions/sleep_node.cpp + src/actions/updated_action.cpp src/decorators/delay_node.cpp - src/decorators/entry_updated_nodes.cpp src/decorators/inverter_node.cpp src/decorators/repeat_node.cpp src/decorators/retry_node.cpp src/decorators/subtree_node.cpp src/decorators/timeout_node.cpp + src/decorators/updated_decorator.cpp src/controls/if_then_else_node.cpp src/controls/fallback_node.cpp diff --git a/include/behaviortree_cpp/actions/updated_action.h b/include/behaviortree_cpp/actions/updated_action.h new file mode 100644 index 000000000..80503ccf3 --- /dev/null +++ b/include/behaviortree_cpp/actions/updated_action.h @@ -0,0 +1,45 @@ +/* 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/action_node.h" + +namespace BT +{ +/** + * @brief The EntryUpdatedAction checks the Timestamp in an entry + * to determine if the value was updated since the last time. + * + * SUCCESS if it was updated, since the last time it was checked, + * FAILURE if it doesn't exist or was not updated. + */ +class EntryUpdatedAction : public SyncActionNode +{ +public: + EntryUpdatedAction(const std::string& name, const NodeConfig& config); + + ~EntryUpdatedAction() override = default; + + static PortsList providedPorts() + { + return { InputPort("entry", "Entry to check") }; + } + +private: + uint64_t sequence_id_ = 0; + std::string entry_key_; + + NodeStatus tick() override; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index 008219cb7..42b860f99 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -33,7 +33,7 @@ #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/entry_updated_nodes.h" +#include "behaviortree_cpp/decorators/updated_decorator.h" #include "behaviortree_cpp/actions/always_success_node.h" #include "behaviortree_cpp/actions/always_failure_node.h" @@ -43,6 +43,7 @@ #include "behaviortree_cpp/actions/test_node.h" #include "behaviortree_cpp/actions/sleep_node.h" #include "behaviortree_cpp/actions/unset_blackboard_node.h" +#include "behaviortree_cpp/actions/updated_action.h" #include "behaviortree_cpp/decorators/force_success_node.h" #include "behaviortree_cpp/decorators/force_failure_node.h" diff --git a/include/behaviortree_cpp/decorators/entry_updated_nodes.h b/include/behaviortree_cpp/decorators/updated_decorator.h similarity index 83% rename from include/behaviortree_cpp/decorators/entry_updated_nodes.h rename to include/behaviortree_cpp/decorators/updated_decorator.h index d9d20c4b0..d570aafb2 100644 --- a/include/behaviortree_cpp/decorators/entry_updated_nodes.h +++ b/include/behaviortree_cpp/decorators/updated_decorator.h @@ -17,19 +17,19 @@ namespace BT { /** - * @brief The EntryUpdatedNode checks the Timestamp in an entry + * @brief The EntryUpdatedDecorator 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 [if_not_updated] value is returned. */ -class EntryUpdatedNode : public DecoratorNode +class EntryUpdatedDecorator : public DecoratorNode { public: - EntryUpdatedNode(const std::string& name, const NodeConfig& config, - NodeStatus if_not_updated); + EntryUpdatedDecorator(const std::string& name, const NodeConfig& config, + NodeStatus if_not_updated); - ~EntryUpdatedNode() override = default; + ~EntryUpdatedDecorator() override = default; static PortsList providedPorts() { @@ -37,7 +37,7 @@ class EntryUpdatedNode : public DecoratorNode } private: - int64_t sequence_id_ = -1; + uint64_t sequence_id_ = 0; std::string entry_key_; bool still_executing_child_ = false; NodeStatus if_not_updated_; diff --git a/include/behaviortree_cpp/scripting/operators.hpp b/include/behaviortree_cpp/scripting/operators.hpp index 176fa2df0..c40da8726 100644 --- a/include/behaviortree_cpp/scripting/operators.hpp +++ b/include/behaviortree_cpp/scripting/operators.hpp @@ -529,7 +529,7 @@ struct ExprAssignment : ExprBase auto value = rhs->evaluate(env); std::scoped_lock lock(entry->entry_mutex); - auto dst_ptr = &entry->value; + auto* dst_ptr = &entry->value; auto errorPrefix = [dst_ptr, &key]() { return StrCat("Error assigning a value to entry [", key, "] with type [", @@ -588,6 +588,8 @@ struct ExprAssignment : ExprBase throw RuntimeError(msg); } } + entry->sequence_id++; + entry->stamp = std::chrono::steady_clock::now().time_since_epoch(); return *dst_ptr; } @@ -642,6 +644,8 @@ struct ExprAssignment : ExprBase } temp_variable.copyInto(*dst_ptr); + entry->sequence_id++; + entry->stamp = std::chrono::steady_clock::now().time_since_epoch(); return *dst_ptr; } }; diff --git a/src/actions/updated_action.cpp b/src/actions/updated_action.cpp new file mode 100644 index 000000000..15be65600 --- /dev/null +++ b/src/actions/updated_action.cpp @@ -0,0 +1,69 @@ +/* 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/actions/updated_action.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace BT +{ + +EntryUpdatedAction::EntryUpdatedAction(const std::string& name, const NodeConfig& config) + : SyncActionNode(name, config) +{ + auto it = config.input_ports.find("entry"); + if(it == config.input_ports.end() || it->second.empty()) + { + throw LogicError("Missing port 'entry' in ", name); + } + const auto entry_str = it->second; + StringView stripped_key; + if(isBlackboardPointer(entry_str, &stripped_key)) + { + entry_key_ = stripped_key; + } + else + { + entry_key_ = entry_str; + } +} + +NodeStatus EntryUpdatedAction::tick() +{ + if(auto entry = config().blackboard->getEntry(entry_key_)) + { + std::unique_lock lk(entry->entry_mutex); + const uint64_t current_id = entry->sequence_id; + const uint64_t previous_id = sequence_id_; + sequence_id_ = current_id; + /* + uint64_t previous_id = 0; + auto& previous_id_registry = details::GlobalSequenceRegistry(); + + // find the previous id in the registry. + auto it = previous_id_registry.find(entry.get()); + if(it != previous_id_registry.end()) + { + previous_id = it->second; + } + if(previous_id != current_id) + { + previous_id_registry[entry.get()] = current_id; + }*/ + return (previous_id != current_id) ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + } + else + { + return NodeStatus::FAILURE; + } +} + +} // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 964954773..c130d4ac7 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -95,8 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType>("LoopDouble"); registerNodeType>("LoopString"); - registerNodeType("SkipUnlessUpdated", NodeStatus::SKIPPED); - registerNodeType("WaitValueUpdate", NodeStatus::RUNNING); + registerNodeType("WasEntryUpdated"); + registerNodeType("SkipUnlessUpdated", NodeStatus::SKIPPED); + registerNodeType("WaitValueUpdate", NodeStatus::RUNNING); for(const auto& it : _p->builders) { diff --git a/src/decorators/entry_updated_nodes.cpp b/src/decorators/updated_decorator.cpp similarity index 68% rename from src/decorators/entry_updated_nodes.cpp rename to src/decorators/updated_decorator.cpp index 20dfd520d..b4c9dd764 100644 --- a/src/decorators/entry_updated_nodes.cpp +++ b/src/decorators/updated_decorator.cpp @@ -10,16 +10,23 @@ * 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/entry_updated_nodes.h" +#include "behaviortree_cpp/decorators/updated_decorator.h" +#include "behaviortree_cpp/bt_factory.h" namespace BT { -EntryUpdatedNode::EntryUpdatedNode(const std::string& name, const NodeConfig& config, - NodeStatus if_not_updated) +EntryUpdatedDecorator::EntryUpdatedDecorator(const std::string& name, + const NodeConfig& config, + NodeStatus if_not_updated) : DecoratorNode(name, config), if_not_updated_(if_not_updated) { - const auto entry_str = config.input_ports.at("entry"); + auto it = config.input_ports.find("entry"); + if(it == config.input_ports.end() || it->second.empty()) + { + throw LogicError("Missing port 'entry' in ", name); + } + const auto entry_str = it->second; StringView stripped_key; if(isBlackboardPointer(entry_str, &stripped_key)) { @@ -31,7 +38,7 @@ EntryUpdatedNode::EntryUpdatedNode(const std::string& name, const NodeConfig& co } } -NodeStatus EntryUpdatedNode::tick() +NodeStatus EntryUpdatedDecorator::tick() { // continue executing an asynchronous child if(still_executing_child_) @@ -41,15 +48,21 @@ NodeStatus EntryUpdatedNode::tick() return status; } + if(auto entry = config().blackboard->getEntry(entry_key_)) { - 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_) + const uint64_t current_id = entry->sequence_id; + const uint64_t previous_id = sequence_id_; + sequence_id_ = current_id; + + if(previous_id == current_id) { return if_not_updated_; } - sequence_id_ = seq; + } + else + { + return if_not_updated_; } auto status = child()->executeTick(); @@ -57,7 +70,7 @@ NodeStatus EntryUpdatedNode::tick() return status; } -void EntryUpdatedNode::halt() +void EntryUpdatedDecorator::halt() { still_executing_child_ = false; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 114f2c55d..738f352e8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -26,6 +26,7 @@ set(BT_TESTS gtest_subtree.cpp gtest_switch.cpp gtest_tree.cpp + gtest_updates.cpp gtest_wakeup.cpp script_parser_test.cpp diff --git a/tests/gtest_updates.cpp b/tests/gtest_updates.cpp new file mode 100644 index 000000000..c77298c87 --- /dev/null +++ b/tests/gtest_updates.cpp @@ -0,0 +1,129 @@ +#include +#include +#include "behaviortree_cpp/basic_types.h" +#include "behaviortree_cpp/bt_factory.h" +#include "test_helper.hpp" + +using namespace BT; + +const std::string xml_text_check = R"( + + + + + + + + + + + + + + + + + )"; + +TEST(EntryUpdates, NoEntry) +{ + BehaviorTreeFactory factory; + std::array counters; + RegisterTestTick(factory, "Test", counters); + + const std::string xml_text = R"( + + + + + + + )"; + + factory.registerBehaviorTreeFromText(xml_text_check); + factory.registerBehaviorTreeFromText(xml_text); + auto tree = factory.createTree("Main"); + const auto status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + EXPECT_EQ(1, counters[0]); // fallback! + EXPECT_EQ(0, counters[1]); // skipped +} + +TEST(EntryUpdates, Initialized) +{ + BehaviorTreeFactory factory; + std::array counters; + RegisterTestTick(factory, "Test", counters); + + const std::string xml_text = R"( + + + +