Control node and Decorators RUNNING before first child

Author: facontidavide

include/behaviortree_cpp/controls/fallback_node.h

@@ -40,6 +40,7 @@ class FallbackNode : public ControlNode
 
 private:
   size_t current_child_idx_;
+  bool all_skipped_ = true;
 
   virtual BT::NodeStatus tick() override;
 };

include/behaviortree_cpp/controls/parallel_node.h

@@ -67,6 +67,7 @@ class ParallelNode : public ControlNode
   int failure_threshold_;
 
   std::set<size_t> skip_list_;
+  bool all_skipped_ = true;
 
   bool read_parameter_from_ports_;
   static constexpr const char* THRESHOLD_SUCCESS = "success_count";

include/behaviortree_cpp/controls/sequence_node.h

@@ -41,6 +41,7 @@ class SequenceNode : public ControlNode
 
 private:
   size_t current_child_idx_;
+  bool all_skipped_ = true;
 
   virtual BT::NodeStatus tick() override;
 };

include/behaviortree_cpp/controls/sequence_star_node.h

@@ -42,6 +42,7 @@ class SequenceWithMemory : public ControlNode
 
 private:
   size_t current_child_idx_;
+  bool all_skipped_ = true;
 
   virtual BT::NodeStatus tick() override;
 };

include/behaviortree_cpp/decorators/delay_node.h

@@ -49,7 +49,7 @@ class DelayNode : public DecoratorNode
   virtual BT::NodeStatus tick() override;
 
   bool delay_started_;
-  bool delay_complete_;
+  std::atomic_bool delay_complete_;
   bool delay_aborted_;
   unsigned msec_;
   bool read_parameter_from_ports_;

include/behaviortree_cpp/decorators/repeat_node.h

@@ -50,6 +50,7 @@ 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";

include/behaviortree_cpp/decorators/retry_node.h

@@ -56,6 +56,7 @@ 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";

include/behaviortree_cpp/decorators/subtree_node.h

@@ -56,13 +56,26 @@ class SubTreeNode : public DecoratorNode
 
   static PortsList providedPorts();
 
-private:
+  void setSubtreeID(const std::string& ID)
+  {
+    subtree_id_ = ID;
+  }
+
+  const std::string& subtreeID() const
+  {
+    return subtree_id_;
+  }
   virtual BT::NodeStatus tick() override;
 
   virtual NodeType type() const override final
   {
     return NodeType::SUBTREE;
   }
+
+
+  private:
+  std::string subtree_id_;
+
 };
 
 }   // namespace BT

src/controls/fallback_node.cpp

@@ -16,7 +16,7 @@
 namespace BT
 {
 FallbackNode::FallbackNode(const std::string& name) :
-  ControlNode::ControlNode(name, {}), current_child_idx_(0)
+  ControlNode::ControlNode(name, {}), current_child_idx_(0), all_skipped_(true)
 {
   setRegistrationID("Fallback");
 }
@@ -25,6 +25,13 @@ NodeStatus FallbackNode::tick()
 {
   const size_t children_count = children_nodes_.size();
 
+  if(status() == NodeStatus::IDLE)
+  {
+    all_skipped_ = true;
+  }
+
+  setStatus(NodeStatus::RUNNING);
+
   while (current_child_idx_ < children_count)
   {
     TreeNode* current_child_node = children_nodes_[current_child_idx_];
@@ -33,10 +40,7 @@ NodeStatus FallbackNode::tick()
     const NodeStatus child_status = current_child_node->executeTick();
 
     // switch to RUNNING state as soon as you find an active child
-    if (child_status != NodeStatus::SKIPPED)
-    {
-      setStatus(NodeStatus::RUNNING);
-    }
+    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
 
     switch (child_status)
     {
@@ -79,7 +83,7 @@ NodeStatus FallbackNode::tick()
   }
 
   // Skip if ALL the nodes have been skipped
-  return status() == (NodeStatus::RUNNING) ? NodeStatus::FAILURE : NodeStatus::SKIPPED;
+  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::FAILURE;
 }
 
 void FallbackNode::halt()

src/controls/parallel_node.cpp

@@ -67,6 +67,12 @@ NodeStatus ParallelNode::tick()
     throw LogicError("Number of children is less than threshold. Can never fail.");
   }
 
+  if(status() == NodeStatus::IDLE)
+  {
+    all_skipped_ = true;
+  }
+  setStatus(NodeStatus::RUNNING);
+
   // Routing the tree according to the sequence node's logic:
   for (size_t i = 0; i < children_count; i++)
   {
@@ -79,10 +85,7 @@ NodeStatus ParallelNode::tick()
         in_skip_list ? prev_status : child_node->executeTick();
 
     // switch to RUNNING state as soon as you find an active child
-    if (child_status != NodeStatus::SKIPPED)
-    {
-      setStatus(NodeStatus::RUNNING);
-    }
+    all_skipped_ &= (child_status == NodeStatus::SKIPPED);
 
     switch (child_status)
     {
@@ -131,7 +134,7 @@ NodeStatus ParallelNode::tick()
     }
   }
   // Skip if ALL the nodes have been skipped
-  return status() == (NodeStatus::RUNNING) ? NodeStatus::RUNNING : NodeStatus::SKIPPED;
+  return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::RUNNING;
 }
 
 void ParallelNode::halt()