Fix Precondition to only check condition once

BehaviorTree · Dec 27, 2024 · e57b4c9 · e57b4c9
1 parent d4d8ae1
commit e57b4c9
Show file tree

Hide file tree

Showing 2 changed files with 122 additions and 37 deletions.
diff --git a/include/behaviortree_cpp/decorators/script_precondition.h b/include/behaviortree_cpp/decorators/script_precondition.h
@@ -48,20 +48,27 @@ class PreconditionNode : public DecoratorNode
       throw RuntimeError("Missing parameter [else] in Precondition");
     }
 
-    Ast::Environment env = { config().blackboard, config().enums };
-    if(_executor(env).cast<bool>())
+    // Only check the 'if' script if we haven't started ticking the children yet.
+    bool tick_children = _children_running;
+    if(!_children_running)
     {
-      auto const child_status = child_node_->executeTick();
-      if(isStatusCompleted(child_status))
-      {
-        resetChild();
-      }
-      return child_status;
+      Ast::Environment env = { config().blackboard, config().enums };
+      tick_children = _executor(env).cast<bool>();
+      _children_running = tick_children;
     }
-    else
+
+    if(!tick_children)
     {
       return else_return;
     }
+
+    auto const child_status = child_node_->executeTick();
+    if(isStatusCompleted(child_status))
+    {
+      resetChild();
+      _children_running = false;
+    }
+    return child_status;
   }
 
   void loadExecutor()
@@ -89,6 +96,7 @@ class PreconditionNode : public DecoratorNode
 
   std::string _script;
   ScriptFunction _executor;
+  bool _children_running = false;
 };
 
 }  // namespace BT
diff --git a/tests/gtest_preconditions.cpp b/tests/gtest_preconditions.cpp
@@ -107,6 +107,111 @@ TEST(PreconditionsDecorator, StringEquals)
   ASSERT_EQ(counters[1], 1);
 }
 
+class KeepRunning : public BT::StatefulActionNode
+{
+public:
+  KeepRunning(const std::string& name, const BT::NodeConfig& config)
+    : BT::StatefulActionNode(name, config)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+  BT::NodeStatus onStart() override
+  {
+    return BT::NodeStatus::RUNNING;
+  }
+
+  BT::NodeStatus onRunning() override
+  {
+    return BT::NodeStatus::RUNNING;
+  }
+
+  void onHalted() override
+  {
+    std::cout << "Node halted\n";
+  }
+};
+
+TEST(PreconditionsDecorator, ChecksConditionOnce)
+{
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<KeepRunning>("KeepRunning");
+
+  const std::string xml_text = R"(
+
+    <root BTCPP_format="4" >
+        <BehaviorTree ID="MainTree">
+            <Sequence>
+                <Script code = "A:=0" />
+                <Script code = "B:=0" />
+                <Precondition if=" A==0 " else="FAILURE">
+                    <KeepRunning _while="B==0" />
+                </Precondition>
+            </Sequence>
+        </BehaviorTree>
+    </root>)";
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING);
+  // While the child is still running, attempt to fail the precondition.
+  tree.rootBlackboard()->set("A", 1);
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING);
+  // Finish running the tree, the else condition should not be hit.
+  tree.rootBlackboard()->set("B", 1);
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS);
+}
+
+TEST(PreconditionsDecorator, CanRunChildrenMultipleTimes)
+{
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<KeepRunning>("KeepRunning");
+  std::array<int, 1> counters;
+  RegisterTestTick(factory, "Test", counters);
+
+  const std::string xml_text = R"(
+
+    <root BTCPP_format="4" >
+        <BehaviorTree ID="MainTree">
+            <Sequence>
+                <Script code = "A:=0" />
+                <Script code = "B:=0" />
+                <Script code = "C:=1" />
+                <Repeat num_cycles="3">
+                    <Sequence>
+                        <Precondition if=" A==0 " else="SUCCESS">
+                            <TestA/>
+                        </Precondition>
+                        <KeepRunning _while="C==0" />
+                        <KeepRunning _while="B==0" />
+                    </Sequence>
+                </Repeat>
+            </Sequence>
+        </BehaviorTree>
+    </root>)";
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING);
+  EXPECT_EQ(counters[0], 1);  // Precondition hit once;
+
+  // In the second repeat, fail the precondition
+  tree.rootBlackboard()->set("A", 1);
+  tree.rootBlackboard()->set("B", 1);
+  tree.rootBlackboard()->set("C", 0);
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING);
+  EXPECT_EQ(counters[0], 1);  // Precondition still only hit once.
+
+  // Finally in the last repeat, hit the condition again.
+  tree.rootBlackboard()->set("A", 0);
+  tree.rootBlackboard()->set("C", 1);
+  EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS);
+  EXPECT_EQ(counters[0], 2);  // Precondition hit twice now.
+}
+
 TEST(Preconditions, Basic)
 {
   BehaviorTreeFactory factory;
@@ -246,34 +351,6 @@ TEST(Preconditions, Issue615_NoSkipWhenRunning_A)
   ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING);
 }
 
-class KeepRunning : public BT::StatefulActionNode
-{
-public:
-  KeepRunning(const std::string& name, const BT::NodeConfig& config)
-    : BT::StatefulActionNode(name, config)
-  {}
-
-  static BT::PortsList providedPorts()
-  {
-    return {};
-  }
-
-  BT::NodeStatus onStart() override
-  {
-    return BT::NodeStatus::RUNNING;
-  }
-
-  BT::NodeStatus onRunning() override
-  {
-    return BT::NodeStatus::RUNNING;
-  }
-
-  void onHalted() override
-  {
-    std::cout << "Node halted\n";
-  }
-};
-
 TEST(Preconditions, Issue615_NoSkipWhenRunning_B)
 {
   static constexpr auto xml_text = R"(