From 36865847749eadde0530000cd9ed6c23b3e282f0 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 18 Apr 2024 14:09:18 +0200 Subject: [PATCH] updated interface with Expected --- include/behaviortree_cpp/basic_types.h | 4 +-- include/behaviortree_cpp/blackboard.h | 40 +++++++++++++++----------- include/behaviortree_cpp/tree_node.h | 36 +++++++++++++++++++++-- src/decorators/skip_unless_updated.cpp | 2 -- tests/gtest_blackboard.cpp | 39 +++++++++++++++++++++++++ 5 files changed, 96 insertions(+), 25 deletions(-) diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index 04fe878dc..580036991 100644 --- a/include/behaviortree_cpp/basic_types.h +++ b/include/behaviortree_cpp/basic_types.h @@ -335,11 +335,9 @@ using Result = Expected; struct Timestamp { uint64_t seq = 0; - std::chrono::nanoseconds stamp = std::chrono::nanoseconds(0); + std::chrono::nanoseconds time = 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 3be6d3444..5073fafb1 100644 --- a/include/behaviortree_cpp/blackboard.h +++ b/include/behaviortree_cpp/blackboard.h @@ -4,7 +4,6 @@ #include #include #include -#include #include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/contrib/json.hpp" @@ -19,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. @@ -83,7 +89,7 @@ class Blackboard [[nodiscard]] bool get(const std::string& key, T& value) const; template - std::optional getStamped(const std::string& key, T& value) const; + [[nodiscard]] Expected getStamped(const std::string& key, T& value) const; /** * Version of get() that throws if it fails. @@ -92,7 +98,7 @@ class Blackboard [[nodiscard]] T get(const std::string& key) const; template - std::pair getStamped(const std::string& key) const; + [[nodiscard]] Expected> getStamped(const std::string& key) const; /// Update the entry with the given key template @@ -307,36 +313,36 @@ inline bool Blackboard::get(const std::string& key, T& value) const } template -inline std::optional Blackboard::getStamped(const std::string& key, - T& value) const +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 std::nullopt; + 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 std::nullopt; + return nonstd::make_unexpected( + StrCat("Blackboard::getStamped() error. Missing key [", key, "]")); } template -inline std::pair Blackboard::getStamped(const std::string& key) const +inline Expected> Blackboard::getStamped(const std::string& key) const { - if(auto entry = getEntry(key)) + StampedValue out; + if(auto res = getStamped(key, out.value)) { - 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 } }; + out.stamp = *res; + return out; + } + else + { + return nonstd::make_unexpected(res.error()); } - 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 ca38916e0..344b04427 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -218,13 +218,23 @@ 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 - ResultStamped getInputStamped(const std::string& key, T& destination) const; + [[nodiscard]] Expected getInputStamped(const std::string& key, + T& destination) const; /** Same as bool getInput(const std::string& key, T& destination) * but using optional. @@ -239,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. @@ -393,8 +423,8 @@ T TreeNode::parseString(const std::string& str) const } template -inline ResultStamped TreeNode::getInputStamped(const std::string& key, - T& destination) const +inline Expected TreeNode::getInputStamped(const std::string& key, + T& destination) const { std::string port_value_str; diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/skip_unless_updated.cpp index 619052bc5..b0865fc3f 100644 --- a/src/decorators/skip_unless_updated.cpp +++ b/src/decorators/skip_unless_updated.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/skip_unless_updated.h" 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); +}