updated interface with Expected

Author: facontidavide

include/behaviortree_cpp/basic_types.h

@@ -335,11 +335,9 @@ using Result = Expected<std::monostate>;
 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<Timestamp>;
-
 [[nodiscard]] bool IsAllowedPortName(StringView str);
 
 class TypeInfo

include/behaviortree_cpp/blackboard.h

@@ -4,7 +4,6 @@
 #include <memory>
 #include <unordered_map>
 #include <mutex>
-#include <optional>
 
 #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<Any>;
 
+template <typename T>
+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 <typename T>
-  std::optional<Timestamp> getStamped(const std::string& key, T& value) const;
+  [[nodiscard]] Expected<Timestamp> 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 <typename T>
-  std::pair<T, Timestamp> getStamped(const std::string& key) const;
+  [[nodiscard]] Expected<StampedValue<T>> getStamped(const std::string& key) const;
 
   /// Update the entry with the given key
   template <typename T>
@@ -307,36 +313,36 @@ inline bool Blackboard::get(const std::string& key, T& value) const
 }
 
 template <typename T>
-inline std::optional<Timestamp> Blackboard::getStamped(const std::string& key,
-                                                       T& value) const
+inline Expected<Timestamp> Blackboard::getStamped(const std::string& key, T& value) const
 {
   if(auto entry = getEntry(key))
   {
     std::unique_lock lk(entry->entry_mutex);
     if(entry->value.empty())
     {
-      return std::nullopt;
+      return nonstd::make_unexpected(StrCat("Blackboard::getStamped() error. Entry [",
+                                            key, "] hasn't been initialized, yet"));
     }
     value = entry->value.cast<T>();
     return Timestamp{ entry->sequence_id, entry->stamp };
   }
-  return std::nullopt;
+  return nonstd::make_unexpected(
+      StrCat("Blackboard::getStamped() error. Missing key [", key, "]"));
 }
 
 template <typename T>
-inline std::pair<T, Timestamp> Blackboard::getStamped(const std::string& key) const
+inline Expected<StampedValue<T>> Blackboard::getStamped(const std::string& key) const
 {
-  if(auto entry = getEntry(key))
+  StampedValue<T> out;
+  if(auto res = getStamped<T>(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<T>(), 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

include/behaviortree_cpp/tree_node.h

@@ -218,13 +218,23 @@ class TreeNode
    * convertFromString<T>() 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 <typename T>
   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 <typename T>
-  ResultStamped getInputStamped(const std::string& key, T& destination) const;
+  [[nodiscard]] Expected<Timestamp> 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<T>(out) : nonstd::make_unexpected(res.error());
   }
 
+  /** Same as bool getInputStamped(const std::string& key, T& destination)
+   * but return StampedValue<T>
+   *
+   * @param key   the name of the port.
+   */
+  template <typename T>
+  [[nodiscard]] Expected<StampedValue<T>> getInputStamped(const std::string& key) const
+  {
+    StampedValue<T> 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 <typename T>
-inline ResultStamped TreeNode::getInputStamped(const std::string& key,
-                                               T& destination) const
+inline Expected<Timestamp> TreeNode::getInputStamped(const std::string& key,
+                                                     T& destination) const
 {
   std::string port_value_str;
 

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

tests/gtest_blackboard.cpp

@@ -606,3 +606,42 @@ TEST(BlackboardTest, RootBlackboard)
   ASSERT_EQ(3, tree.rootBlackboard()->get<int>("var3"));
   ASSERT_EQ(4, tree.rootBlackboard()->get<int>("var4"));
 }
+
+TEST(BlackboardTest, TimestampedInterface)
+{
+  auto bb = BT::Blackboard::create();
+
+  // still empty, expected to fail
+  int value;
+  ASSERT_FALSE(bb->getStamped<int>("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<int>("value");
+  auto stamp_opt = bb->getStamped<int>("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<int>("value");
+  stamp_opt = bb->getStamped<int>("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);
+}