bug fixes related to sequence_id and unit tests added

Author: facontidavide

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

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<BT::Any>("entry", "Entry to check") };
+  }
+
+private:
+  uint64_t sequence_id_ = 0;
+  std::string entry_key_;
+
+  NodeStatus tick() override;
+};
+
+}  // namespace BT

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"

include/behaviortree_cpp/decorators/entry_updated_nodes.h renamed to include/behaviortree_cpp/decorators/updated_decorator.h

@@ -17,27 +17,27 @@
 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()
   {
     return { InputPort<BT::Any>("entry", "Entry to check") };
   }
 
 private:
-  int64_t sequence_id_ = -1;
+  uint64_t sequence_id_ = 0;
   std::string entry_key_;
   bool still_executing_child_ = false;
   NodeStatus if_not_updated_;

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;
   }
 };

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

src/bt_factory.cpp

@@ -95,8 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<LoopNode<double>>("LoopDouble");
   registerNodeType<LoopNode<std::string>>("LoopString");
 
-  registerNodeType<EntryUpdatedNode>("SkipUnlessUpdated", NodeStatus::SKIPPED);
-  registerNodeType<EntryUpdatedNode>("WaitValueUpdate", NodeStatus::RUNNING);
+  registerNodeType<EntryUpdatedAction>("WasEntryUpdated");
+  registerNodeType<EntryUpdatedDecorator>("SkipUnlessUpdated", NodeStatus::SKIPPED);
+  registerNodeType<EntryUpdatedDecorator>("WaitValueUpdate", NodeStatus::RUNNING);
 
   for(const auto& it : _p->builders)
   {

src/decorators/entry_updated_nodes.cpp renamed to 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,23 +48,29 @@ 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<int64_t>(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();
   still_executing_child_ = (status == NodeStatus::RUNNING);
   return status;
 }
 
-void EntryUpdatedNode::halt()
+void EntryUpdatedDecorator::halt()
 {
   still_executing_child_ = false;
 }

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