fix the "all_skipped" logic

facontidavide · facontidavide · commit 957a7f88029f · 2024-09-13T18:29:42.000+02:00
diff --git a/include/behaviortree_cpp/controls/fallback_node.h b/include/behaviortree_cpp/controls/fallback_node.h
@@ -40,7 +40,7 @@ class FallbackNode : public ControlNode
 
 private:
   size_t current_child_idx_;
-  bool all_skipped_ = true;
+  size_t skipped_count_ = 0;
   bool asynch_ = false;
 
   virtual BT::NodeStatus tick() override;
diff --git a/include/behaviortree_cpp/controls/sequence_node.h b/include/behaviortree_cpp/controls/sequence_node.h
@@ -42,7 +42,7 @@ class SequenceNode : public ControlNode
 
 private:
   size_t current_child_idx_;
-  bool all_skipped_ = true;
+  size_t skipped_count_ = 0;
   bool asynch_ = false;
 
   virtual BT::NodeStatus tick() override;
diff --git a/include/behaviortree_cpp/controls/sequence_with_memory_node.h b/include/behaviortree_cpp/controls/sequence_with_memory_node.h
@@ -42,7 +42,7 @@ class SequenceWithMemory : public ControlNode
 
 private:
   size_t current_child_idx_;
-  bool all_skipped_ = true;
+  size_t skipped_count_ = 0;
 
   virtual BT::NodeStatus tick() override;
 };
diff --git a/include/behaviortree_cpp/decorators/repeat_node.h b/include/behaviortree_cpp/decorators/repeat_node.h
@@ -50,7 +50,6 @@ class RepeatNode : public DecoratorNode
 private:
   int num_cycles_;
   int repeat_count_;
-  bool all_skipped_ = true;
 
   bool read_parameter_from_ports_;
   static constexpr const char* NUM_CYCLES = "num_cycles";
diff --git a/include/behaviortree_cpp/decorators/retry_node.h b/include/behaviortree_cpp/decorators/retry_node.h
@@ -56,7 +56,6 @@ class RetryNode : public DecoratorNode
 private:
   int max_attempts_;
   int try_count_;
-  bool all_skipped_ = true;
 
   bool read_parameter_from_ports_;
   static constexpr const char* NUM_ATTEMPTS = "num_attempts";
diff --git a/src/controls/fallback_node.cpp b/src/controls/fallback_node.cpp
@@ -18,7 +18,6 @@ namespace BT
 FallbackNode::FallbackNode(const std::string& name, bool make_asynch)
   : ControlNode::ControlNode(name, {})
   , current_child_idx_(0)
-  , all_skipped_(true)
   , asynch_(make_asynch)
 {
   if(asynch_)
@@ -33,7 +32,7 @@ NodeStatus FallbackNode::tick()
 
   if(status() == NodeStatus::IDLE)
   {
-    all_skipped_ = true;
+    skipped_count_ = 0;
   }
 
   setStatus(NodeStatus::RUNNING);
@@ -45,9 +44,6 @@ NodeStatus FallbackNode::tick()
     auto prev_status = current_child_node->status();
     const NodeStatus child_status = current_child_node->executeTick();
 
-    // switch to RUNNING state as soon as you find an active child
-    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
-
     switch(child_status)
     {
       case NodeStatus::RUNNING: {
@@ -73,6 +69,7 @@ NodeStatus FallbackNode::tick()
       case NodeStatus::SKIPPED: {
         // It was requested to skip this node
         current_child_idx_++;
+        skipped_count_++;
       }
       break;
       case NodeStatus::IDLE: {
@@ -89,7 +86,7 @@ NodeStatus FallbackNode::tick()
   }
 
   // Skip if ALL the nodes have been skipped
-  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::FAILURE;
+  return (skipped_count_ == children_count) ? NodeStatus::SKIPPED : NodeStatus::FAILURE;
 }
 
 void FallbackNode::halt()
diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp
@@ -18,7 +18,6 @@ namespace BT
 SequenceNode::SequenceNode(const std::string& name, bool make_async)
   : ControlNode::ControlNode(name, {})
   , current_child_idx_(0)
-  , all_skipped_(true)
   , asynch_(make_async)
 {
   if(asynch_)
@@ -39,7 +38,7 @@ NodeStatus SequenceNode::tick()
 
   if(status() == NodeStatus::IDLE)
   {
-    all_skipped_ = true;
+    skipped_count_ = 0;
   }
 
   setStatus(NodeStatus::RUNNING);
@@ -51,9 +50,6 @@ NodeStatus SequenceNode::tick()
     auto prev_status = current_child_node->status();
     const NodeStatus child_status = current_child_node->executeTick();
 
-    // switch to RUNNING state as soon as you find an active child
-    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
-
     switch(child_status)
     {
       case NodeStatus::RUNNING: {
@@ -81,6 +77,7 @@ NodeStatus SequenceNode::tick()
       case NodeStatus::SKIPPED: {
         // It was requested to skip this node
         current_child_idx_++;
+        skipped_count_++;
       }
       break;
 
@@ -97,7 +94,7 @@ NodeStatus SequenceNode::tick()
     current_child_idx_ = 0;
   }
   // Skip if ALL the nodes have been skipped
-  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;
+  return (skipped_count_ == children_count) ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;
 }
 
 }  // namespace BT
diff --git a/src/controls/sequence_with_memory_node.cpp b/src/controls/sequence_with_memory_node.cpp
@@ -27,7 +27,7 @@ NodeStatus SequenceWithMemory::tick()
 
   if(status() == NodeStatus::IDLE)
   {
-    all_skipped_ = true;
+    skipped_count_ = 0;
   }
   setStatus(NodeStatus::RUNNING);
 
@@ -38,9 +38,6 @@ NodeStatus SequenceWithMemory::tick()
     auto prev_status = current_child_node->status();
     const NodeStatus child_status = current_child_node->executeTick();
 
-    // switch to RUNNING state as soon as you find an active child
-    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
-
     switch(child_status)
     {
       case NodeStatus::RUNNING: {
@@ -71,6 +68,7 @@ NodeStatus SequenceWithMemory::tick()
       case NodeStatus::SKIPPED: {
         // It was requested to skip this node
         current_child_idx_++;
+        skipped_count_++;
       }
       break;
 
@@ -87,7 +85,7 @@ NodeStatus SequenceWithMemory::tick()
     current_child_idx_ = 0;
   }
   // Skip if ALL the nodes have been skipped
-  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;
+  return (skipped_count_ == children_count) ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;
 }
 
 void SequenceWithMemory::halt()
diff --git a/src/decorators/repeat_node.cpp b/src/decorators/repeat_node.cpp
@@ -43,20 +43,13 @@ NodeStatus RepeatNode::tick()
   }
 
   bool do_loop = repeat_count_ < num_cycles_ || num_cycles_ == -1;
-  if(status() == NodeStatus::IDLE)
-  {
-    all_skipped_ = true;
-  }
   setStatus(NodeStatus::RUNNING);
 
   while(do_loop)
   {
     NodeStatus const prev_status = child_node_->status();
     NodeStatus child_status = child_node_->executeTick();
 
-    // switch to RUNNING state as soon as you find an active child
-    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
-
     switch(child_status)
     {
       case NodeStatus::SUCCESS: {
@@ -99,7 +92,7 @@ NodeStatus RepeatNode::tick()
   }
 
   repeat_count_ = 0;
-  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;
+  return NodeStatus::SUCCESS;
 }
 
 void RepeatNode::halt()
diff --git a/src/decorators/retry_node.cpp b/src/decorators/retry_node.cpp
@@ -50,21 +50,13 @@ NodeStatus RetryNode::tick()
   }
 
   bool do_loop = try_count_ < max_attempts_ || max_attempts_ == -1;
-
-  if(status() == NodeStatus::IDLE)
-  {
-    all_skipped_ = true;
-  }
   setStatus(NodeStatus::RUNNING);
 
   while(do_loop)
   {
     NodeStatus prev_status = child_node_->status();
     NodeStatus child_status = child_node_->executeTick();
 
-    // switch to RUNNING state as soon as you find an active child
-    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
-
     switch(child_status)
     {
       case NodeStatus::SUCCESS: {
@@ -107,7 +99,7 @@ NodeStatus RetryNode::tick()
   }
 
   try_count_ = 0;
-  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::FAILURE;
+  return NodeStatus::FAILURE;
 }
 
 }  // namespace BT
diff --git a/tests/gtest_preconditions.cpp b/tests/gtest_preconditions.cpp
@@ -363,3 +363,38 @@ TEST(Preconditions, Remapping)
   ASSERT_EQ(counters[0], 1);
   ASSERT_EQ(counters[1], 3);
 }
+
+
+TEST(Preconditions, WhileCallsOnHalt)
+{
+  static constexpr auto xml_text = R"(
+  <root BTCPP_format="4">
+
+    <BehaviorTree ID="Main">
+      <Sequence>
+        <KeepRunning _while="A==1" _onHalted="B=69" />
+      </Sequence>
+    </BehaviorTree>
+  </root>
+  )";
+
+  BehaviorTreeFactory factory;
+
+  factory.registerNodeType<KeepRunning>("KeepRunning");
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("Main");
+
+  tree.rootBlackboard()->set("A", 1);
+  tree.rootBlackboard()->set("B", 0);
+  auto status = tree.tickOnce();
+
+  ASSERT_EQ(status, BT::NodeStatus::RUNNING);
+  ASSERT_EQ(tree.rootBlackboard()->get<int>("B"), 0);
+
+  // trigger halt
+  tree.rootBlackboard()->set("A", 0);
+  status = tree.tickOnce();
+
+  ASSERT_EQ(status, BT::NodeStatus::SKIPPED);
+  ASSERT_EQ(tree.rootBlackboard()->get<int>("B"), 69);
+}

Original file line number	Diff line number	Diff line change
`@@ -18,7 +18,6 @@ namespace BT`
`18`	`18`	`FallbackNode::FallbackNode(const std::string& name, bool make_asynch)`
`19`	`19`	`: ControlNode::ControlNode(name, {})`
`20`	`20`	`, current_child_idx_(0)`
`21`		`- , all_skipped_(true)`
`22`	`21`	`, asynch_(make_asynch)`
`23`	`22`	`{`
`24`	`23`	`if(asynch_)`
`@@ -33,7 +32,7 @@ NodeStatus FallbackNode::tick()`
`33`	`32`
`34`	`33`	`if(status() == NodeStatus::IDLE)`
`35`	`34`	`{`
`36`		`- all_skipped_ = true;`
	`35`	`+ skipped_count_ = 0;`
`37`	`36`	`}`
`38`	`37`
`39`	`38`	`setStatus(NodeStatus::RUNNING);`
`@@ -45,9 +44,6 @@ NodeStatus FallbackNode::tick()`
`45`	`44`	`auto prev_status = current_child_node->status();`
`46`	`45`	`const NodeStatus child_status = current_child_node->executeTick();`
`47`	`46`
`48`		`- // switch to RUNNING state as soon as you find an active child`
`49`		`- all_skipped_ &= (child_status == NodeStatus::SKIPPED);`
`50`		`-`
`51`	`47`	`switch(child_status)`
`52`	`48`	`{`
`53`	`49`	`case NodeStatus::RUNNING: {`
`@@ -73,6 +69,7 @@ NodeStatus FallbackNode::tick()`
`73`	`69`	`case NodeStatus::SKIPPED: {`
`74`	`70`	`// It was requested to skip this node`
`75`	`71`	`current_child_idx_++;`
	`72`	`+ skipped_count_++;`
`76`	`73`	`}`
`77`	`74`	`break;`
`78`	`75`	`case NodeStatus::IDLE: {`
`@@ -89,7 +86,7 @@ NodeStatus FallbackNode::tick()`
`89`	`86`	`}`
`90`	`87`
`91`	`88`	`// Skip if ALL the nodes have been skipped`
`92`		`- return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::FAILURE;`
	`89`	`+ return (skipped_count_ == children_count) ? NodeStatus::SKIPPED : NodeStatus::FAILURE;`
`93`	`90`	`}`
`94`	`91`
`95`	`92`	`void FallbackNode::halt()`
Original file line number	Diff line number	Diff line change
`@@ -27,7 +27,7 @@ NodeStatus SequenceWithMemory::tick()`
`27`	`27`
`28`	`28`	`if(status() == NodeStatus::IDLE)`
`29`	`29`	`{`
`30`		`- all_skipped_ = true;`
	`30`	`+ skipped_count_ = 0;`
`31`	`31`	`}`
`32`	`32`	`setStatus(NodeStatus::RUNNING);`
`33`	`33`
`@@ -38,9 +38,6 @@ NodeStatus SequenceWithMemory::tick()`
`38`	`38`	`auto prev_status = current_child_node->status();`
`39`	`39`	`const NodeStatus child_status = current_child_node->executeTick();`
`40`	`40`
`41`		`- // switch to RUNNING state as soon as you find an active child`
`42`		`- all_skipped_ &= (child_status == NodeStatus::SKIPPED);`
`43`		`-`
`44`	`41`	`switch(child_status)`
`45`	`42`	`{`
`46`	`43`	`case NodeStatus::RUNNING: {`
`@@ -71,6 +68,7 @@ NodeStatus SequenceWithMemory::tick()`
`71`	`68`	`case NodeStatus::SKIPPED: {`
`72`	`69`	`// It was requested to skip this node`
`73`	`70`	`current_child_idx_++;`
	`71`	`+ skipped_count_++;`
`74`	`72`	`}`
`75`	`73`	`break;`
`76`	`74`
`@@ -87,7 +85,7 @@ NodeStatus SequenceWithMemory::tick()`
`87`	`85`	`current_child_idx_ = 0;`
`88`	`86`	`}`
`89`	`87`	`// Skip if ALL the nodes have been skipped`
`90`		`- return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;`
	`88`	`+ return (skipped_count_ == children_count) ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;`
`91`	`89`	`}`
`92`	`90`
`93`	`91`	`void SequenceWithMemory::halt()`
Original file line number	Diff line number	Diff line change
`@@ -43,20 +43,13 @@ NodeStatus RepeatNode::tick()`
`43`	`43`	`}`
`44`	`44`
`45`	`45`	`bool do_loop = repeat_count_ < num_cycles_ \|\| num_cycles_ == -1;`
`46`		`- if(status() == NodeStatus::IDLE)`
`47`		`- {`
`48`		`- all_skipped_ = true;`
`49`		`- }`
`50`	`46`	`setStatus(NodeStatus::RUNNING);`
`51`	`47`
`52`	`48`	`while(do_loop)`
`53`	`49`	`{`
`54`	`50`	`NodeStatus const prev_status = child_node_->status();`
`55`	`51`	`NodeStatus child_status = child_node_->executeTick();`
`56`	`52`
`57`		`- // switch to RUNNING state as soon as you find an active child`
`58`		`- all_skipped_ &= (child_status == NodeStatus::SKIPPED);`
`59`		`-`
`60`	`53`	`switch(child_status)`
`61`	`54`	`{`
`62`	`55`	`case NodeStatus::SUCCESS: {`
`@@ -99,7 +92,7 @@ NodeStatus RepeatNode::tick()`
`99`	`92`	`}`
`100`	`93`
`101`	`94`	`repeat_count_ = 0;`
`102`		`- return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS;`
	`95`	`+ return NodeStatus::SUCCESS;`
`103`	`96`	`}`
`104`	`97`
`105`	`98`	`void RepeatNode::halt()`
Original file line number	Diff line number	Diff line change
`@@ -50,21 +50,13 @@ NodeStatus RetryNode::tick()`
`50`	`50`	`}`
`51`	`51`
`52`	`52`	`bool do_loop = try_count_ < max_attempts_ \|\| max_attempts_ == -1;`
`53`		`-`
`54`		`- if(status() == NodeStatus::IDLE)`
`55`		`- {`
`56`		`- all_skipped_ = true;`
`57`		`- }`
`58`	`53`	`setStatus(NodeStatus::RUNNING);`
`59`	`54`
`60`	`55`	`while(do_loop)`
`61`	`56`	`{`
`62`	`57`	`NodeStatus prev_status = child_node_->status();`
`63`	`58`	`NodeStatus child_status = child_node_->executeTick();`
`64`	`59`
`65`		`- // switch to RUNNING state as soon as you find an active child`
`66`		`- all_skipped_ &= (child_status == NodeStatus::SKIPPED);`
`67`		`-`
`68`	`60`	`switch(child_status)`
`69`	`61`	`{`
`70`	`62`	`case NodeStatus::SUCCESS: {`
`@@ -107,7 +99,7 @@ NodeStatus RetryNode::tick()`
`107`	`99`	`}`
`108`	`100`
`109`	`101`	`try_count_ = 0;`
`110`		`- return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::FAILURE;`
	`102`	`+ return NodeStatus::FAILURE;`
`111`	`103`	`}`
`112`	`104`
`113`	`105`	`} // namespace BT`