Read at every tick the parameter if Blackboard is used

Author: Davide Faconti

include/behaviortree_cpp/controls/sequence_star_node.h

@@ -46,14 +46,17 @@ class SequenceStarNode : public ControlNode
 
     static const NodeParameters& requiredNodeParameters()
     {
-        static NodeParameters params = {{"reset_on_failure", "true"}};
+        static NodeParameters params = {{RESET_PARAM, "true"}};
         return params;
     }
 
   private:
     unsigned int current_child_idx_;
     bool reset_on_failure_;
 
+    bool refresh_parameter_;
+    static constexpr const char* RESET_PARAM = "reset_on_failure";
+
     virtual BT::NodeStatus tick() override;
 };
 }

include/behaviortree_cpp/decorators/repeat_node.h

@@ -35,10 +35,12 @@ class RepeatNode : public DecoratorNode
     }
 
   private:
-    unsigned NTries_;
-    unsigned TryIndx_;
+    unsigned num_cycles_;
+    unsigned try_index_;
 
+    bool refresh_parameter_;
     static constexpr const char* NUM_CYCLES = "num_cycles";
+
     virtual BT::NodeStatus tick() override;
 };
 }

include/behaviortree_cpp/decorators/retry_node.h

@@ -35,10 +35,12 @@ class RetryNode : public DecoratorNode
     }
 
   private:
-    unsigned int NTries_;
-    unsigned int TryIndx_;
+    unsigned int max_attempts_;
+    unsigned int try_index_;
 
+    bool refresh_parameter_;
     static constexpr const char* NUM_ATTEMPTS = "num_attempts";
+
     virtual BT::NodeStatus tick() override;
 };
 }

src/controls/parallel_node.cpp

@@ -28,11 +28,9 @@ ParallelNode::ParallelNode(const std::string& name, int threshold)
 ParallelNode::ParallelNode(const std::string &name,
                                const NodeParameters &params)
     : ControlNode::ControlNode(name, params),
-    threshold_(1),
     refresh_parameter_(false)
   {
-    auto param = getParam<unsigned>( THRESHOLD_KEY );
-    if( !param )
+    if( !getParam(THRESHOLD_KEY, threshold_) )
     {
         throw std::runtime_error("Missing parameter [threshold] in ParallelNode");
     }

src/controls/sequence_star_node.cpp

@@ -15,22 +15,37 @@
 
 namespace BT
 {
+
+constexpr const char* SequenceStarNode::RESET_PARAM;
+
 SequenceStarNode::SequenceStarNode(const std::string& name, bool reset_on_failure)
-  : ControlNode::ControlNode(name, SequenceStarNode::requiredNodeParameters())
+  : ControlNode::ControlNode(name, {{RESET_PARAM, std::to_string(reset_on_failure)}})
   , current_child_idx_(0)
   , reset_on_failure_(reset_on_failure)
+  , refresh_parameter_(false)
 {
 }
 
 SequenceStarNode::SequenceStarNode(const std::string& name, const NodeParameters& params)
-  : ControlNode::ControlNode(name, params), current_child_idx_(0), reset_on_failure_(true)
+  : ControlNode::ControlNode(name, params), current_child_idx_(0),
+    refresh_parameter_(false)
 {
-    getParam<bool>("reset_on_failure", reset_on_failure_);
+    if( !getParam(RESET_PARAM, reset_on_failure_) )
+    {
+        throw std::runtime_error("Missing parameter [reset_on_failure] in SequenceStarNode");
+    }
+    refresh_parameter_ = isBlackboardPattern( params.begin()->second );
 }
 
 NodeStatus SequenceStarNode::tick()
 {
-    // Vector size initialization. children_count_ could change at runtime if you edit the tree
+    if( refresh_parameter_)
+    {
+        // Read it at every tick. Since it points to the blackboard,
+        // it may change dynamically
+        getParam(RESET_PARAM, reset_on_failure_);
+    }
+
     const unsigned children_count = children_nodes_.size();
 
     setStatus(NodeStatus::RUNNING);

src/decorators/repeat_node.cpp

@@ -18,31 +18,43 @@ namespace BT
 constexpr const char* RepeatNode::NUM_CYCLES;
 
 RepeatNode::RepeatNode(const std::string& name, unsigned int NTries)
-  : DecoratorNode(name, {{NUM_CYCLES, std::to_string(NTries)}}), NTries_(NTries), TryIndx_(0)
+  : DecoratorNode(name, {{NUM_CYCLES, std::to_string(NTries)}}),
+    num_cycles_(NTries),
+    try_index_(0),
+    refresh_parameter_(false)
 {
 }
 
 RepeatNode::RepeatNode(const std::string& name, const NodeParameters& params)
-  : DecoratorNode(name, params), NTries_(1), TryIndx_(0)
+  : DecoratorNode(name, params),
+    try_index_(0),
+    refresh_parameter_(false)
 {
-    auto param = getParam<int>(NUM_CYCLES);
-    if (param)
+    if( !getParam(NUM_CYCLES, num_cycles_) )
     {
-        NTries_ = param.value();
+        throw std::runtime_error("Missing parameter [num_cycles] in RepeatNode");
     }
+    refresh_parameter_ = isBlackboardPattern( params.begin()->second );
 }
 
 NodeStatus RepeatNode::tick()
 {
+    if( refresh_parameter_ )
+    {
+        // Read it at every tick. Since it points to the blackboard,
+        // it may change dynamically
+        getParam(RepeatNode::NUM_CYCLES, num_cycles_);
+    }
+
     setStatus(NodeStatus::RUNNING);
     NodeStatus child_state = child_node_->executeTick();
 
     switch (child_state)
     {
         case NodeStatus::SUCCESS:
         {
-            TryIndx_++;
-            if (TryIndx_ >= NTries_)
+            try_index_++;
+            if (try_index_ >= num_cycles_)
             {
                 setStatus(NodeStatus::SUCCESS);
                 child_node_->setStatus(NodeStatus::IDLE);
@@ -52,7 +64,7 @@ NodeStatus RepeatNode::tick()
 
         case NodeStatus::FAILURE:
         {
-            TryIndx_ = 0;
+            try_index_ = 0;
             setStatus(NodeStatus::FAILURE);
             child_node_->setStatus(NodeStatus::IDLE);
         }

src/decorators/retry_node.cpp

@@ -18,41 +18,53 @@ namespace BT
 constexpr const char* RetryNode::NUM_ATTEMPTS;
 
 RetryNode::RetryNode(const std::string& name, unsigned int NTries)
-  : DecoratorNode(name, {{NUM_ATTEMPTS, std::to_string(NTries)}}), NTries_(NTries), TryIndx_(0)
+  : DecoratorNode(name, {{NUM_ATTEMPTS, std::to_string(NTries)}}),
+    max_attempts_(NTries),
+    try_index_(0),
+    refresh_parameter_(false)
 {
 }
 
 RetryNode::RetryNode(const std::string& name, const NodeParameters& params)
-  : DecoratorNode(name, params), NTries_(1), TryIndx_(0)
+  : DecoratorNode(name, params),
+    try_index_(0),
+    refresh_parameter_(false)
 {
-    auto param = getParam<int>(NUM_ATTEMPTS);
-    if (param)
+    if( !getParam(NUM_ATTEMPTS, max_attempts_) )
     {
-        NTries_ = param.value();
+        throw std::runtime_error("Missing parameter [num_attempts] in RetryNode");
     }
+    refresh_parameter_ = isBlackboardPattern( params.begin()->second );
 }
 
 NodeStatus RetryNode::tick()
 {
+    if( refresh_parameter_ )
+    {
+        // Read it at every tick. Since it points to the blackboard,
+        // it may change dynamically
+        getParam(NUM_ATTEMPTS, max_attempts_);
+    }
+
     setStatus(NodeStatus::RUNNING);
     NodeStatus child_state = child_node_->executeTick();
 
     switch (child_state)
     {
         case NodeStatus::SUCCESS:
         {
-            TryIndx_ = 0;
+            try_index_ = 0;
             setStatus(NodeStatus::SUCCESS);
             child_node_->setStatus(NodeStatus::IDLE);
         }
         break;
 
         case NodeStatus::FAILURE:
         {
-            TryIndx_++;
-            if (TryIndx_ >= NTries_)
+            try_index_++;
+            if (try_index_ >= max_attempts_)
             {
-                TryIndx_ = 0;
+                try_index_ = 0;
                 setStatus(NodeStatus::FAILURE);
                 child_node_->setStatus(NodeStatus::IDLE);
             }