new decorators using the entry sequence, added

Author: facontidavide

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
@@ -148,7 +150,8 @@ endif()
 
 if (BTCPP_SHARED_LIBS)
     set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
-    add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE})
+    add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE}
+        src/decorators/wait_update.cpp)
 else()
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
     add_library(${BTCPP_LIBRARY} STATIC ${BT_SOURCE})

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"

include/behaviortree_cpp/bt_factory.h

@@ -14,14 +14,11 @@
 #ifndef BT_FACTORY_H
 #define BT_FACTORY_H
 
-#include <algorithm>
-#include <cstring>
 #include <filesystem>
 #include <functional>
 #include <memory>
 #include <unordered_map>
 #include <set>
-#include <utility>
 #include <vector>
 
 #include "behaviortree_cpp/contrib/magic_enum.hpp"

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<BT::Any>("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

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<BT::Any>("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

include/behaviortree_cpp/tree_node.h

@@ -467,7 +467,7 @@ inline ResultStamped TreeNode::getInputStamped(const std::string& key,
       if constexpr(std::is_same_v<T, Any>)
       {
         destination = any_value;
-        return {};
+        return Timestamp{ entry->sequence_id, entry->stamp };
       }
 
       if(!entry->value.empty())

src/bt_factory.cpp

@@ -95,6 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<LoopNode<double>>("LoopDouble");
   registerNodeType<LoopNode<std::string>>("LoopString");
 
+  registerNodeType<SkipUnlessUpdated>("SkipUnlessUpdated");
+  registerNodeType<WaitValueUpdate>("WaitValueUpdate");
+
   for(const auto& it : _p->builders)
   {
     _p->builtin_IDs.insert(it.first);

src/decorators/skip_unless_updated.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/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<int64_t>(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

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<int64_t>(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