Read at every tick the parameter if Blackboard is used

Davide Faconti · Davide Faconti · commit 17501380ef4a · 2018-11-19T10:51:13.000+01:00
diff --git a/include/behaviortree_cpp/controls/sequence_star_node.h b/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;
 };
 }
diff --git a/include/behaviortree_cpp/decorators/repeat_node.h b/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;
 };
 }
diff --git a/include/behaviortree_cpp/decorators/retry_node.h b/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;
 };
 }
diff --git a/src/controls/parallel_node.cpp b/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");
     }
diff --git a/src/controls/sequence_star_node.cpp b/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);
diff --git a/src/decorators/repeat_node.cpp b/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);
         }
diff --git a/src/decorators/retry_node.cpp b/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);
             }

Original file line number	Diff line number	Diff line change
`@@ -46,14 +46,17 @@ class SequenceStarNode : public ControlNode`
`46`	`46`
`47`	`47`	`static const NodeParameters& requiredNodeParameters()`
`48`	`48`	`{`
`49`		`- static NodeParameters params = {{"reset_on_failure", "true"}};`
	`49`	`+ static NodeParameters params = {{RESET_PARAM, "true"}};`
`50`	`50`	`return params;`
`51`	`51`	`}`
`52`	`52`
`53`	`53`	`private:`
`54`	`54`	`unsigned int current_child_idx_;`
`55`	`55`	`bool reset_on_failure_;`
`56`	`56`
	`57`	`+ bool refresh_parameter_;`
	`58`	`+ static constexpr const char* RESET_PARAM = "reset_on_failure";`
	`59`	`+`
`57`	`60`	`virtual BT::NodeStatus tick() override;`
`58`	`61`	`};`
`59`	`62`	`}`
Original file line number	Diff line number	Diff line change
`@@ -35,10 +35,12 @@ class RepeatNode : public DecoratorNode`
`35`	`35`	`}`
`36`	`36`
`37`	`37`	`private:`
`38`		`- unsigned NTries_;`
`39`		`- unsigned TryIndx_;`
	`38`	`+ unsigned num_cycles_;`
	`39`	`+ unsigned try_index_;`
`40`	`40`
	`41`	`+ bool refresh_parameter_;`
`41`	`42`	`static constexpr const char* NUM_CYCLES = "num_cycles";`
	`43`	`+`
`42`	`44`	`virtual BT::NodeStatus tick() override;`
`43`	`45`	`};`
`44`	`46`	`}`
Original file line number	Diff line number	Diff line change
`@@ -35,10 +35,12 @@ class RetryNode : public DecoratorNode`
`35`	`35`	`}`
`36`	`36`
`37`	`37`	`private:`
`38`		`- unsigned int NTries_;`
`39`		`- unsigned int TryIndx_;`
	`38`	`+ unsigned int max_attempts_;`
	`39`	`+ unsigned int try_index_;`
`40`	`40`
	`41`	`+ bool refresh_parameter_;`
`41`	`42`	`static constexpr const char* NUM_ATTEMPTS = "num_attempts";`
	`43`	`+`
`42`	`44`	`virtual BT::NodeStatus tick() override;`
`43`	`45`	`};`
`44`	`46`	`}`
Original file line number	Diff line number	Diff line change
`@@ -28,11 +28,9 @@ ParallelNode::ParallelNode(const std::string& name, int threshold)`
`28`	`28`	`ParallelNode::ParallelNode(const std::string &name,`
`29`	`29`	`const NodeParameters &params)`
`30`	`30`	`: ControlNode::ControlNode(name, params),`
`31`		`- threshold_(1),`
`32`	`31`	`refresh_parameter_(false)`
`33`	`32`	`{`
`34`		`- auto param = getParam<unsigned>( THRESHOLD_KEY );`
`35`		`- if( !param )`
	`33`	`+ if( !getParam(THRESHOLD_KEY, threshold_) )`
`36`	`34`	`{`
`37`	`35`	`throw std::runtime_error("Missing parameter [threshold] in ParallelNode");`
`38`	`36`	`}`
Original file line number	Diff line number	Diff line change
`@@ -18,31 +18,43 @@ namespace BT`
`18`	`18`	`constexpr const char* RepeatNode::NUM_CYCLES;`
`19`	`19`
`20`	`20`	`RepeatNode::RepeatNode(const std::string& name, unsigned int NTries)`
`21`		`- : DecoratorNode(name, {{NUM_CYCLES, std::to_string(NTries)}}), NTries_(NTries), TryIndx_(0)`
	`21`	`+ : DecoratorNode(name, {{NUM_CYCLES, std::to_string(NTries)}}),`
	`22`	`+ num_cycles_(NTries),`
	`23`	`+ try_index_(0),`
	`24`	`+ refresh_parameter_(false)`
`22`	`25`	`{`
`23`	`26`	`}`
`24`	`27`
`25`	`28`	`RepeatNode::RepeatNode(const std::string& name, const NodeParameters& params)`
`26`		`- : DecoratorNode(name, params), NTries_(1), TryIndx_(0)`
	`29`	`+ : DecoratorNode(name, params),`
	`30`	`+ try_index_(0),`
	`31`	`+ refresh_parameter_(false)`
`27`	`32`	`{`
`28`		`- auto param = getParam<int>(NUM_CYCLES);`
`29`		`- if (param)`
	`33`	`+ if( !getParam(NUM_CYCLES, num_cycles_) )`
`30`	`34`	`{`
`31`		`- NTries_ = param.value();`
	`35`	`+ throw std::runtime_error("Missing parameter [num_cycles] in RepeatNode");`
`32`	`36`	`}`
	`37`	`+ refresh_parameter_ = isBlackboardPattern( params.begin()->second );`
`33`	`38`	`}`
`34`	`39`
`35`	`40`	`NodeStatus RepeatNode::tick()`
`36`	`41`	`{`
	`42`	`+ if( refresh_parameter_ )`
	`43`	`+ {`
	`44`	`+ // Read it at every tick. Since it points to the blackboard,`
	`45`	`+ // it may change dynamically`
	`46`	`+ getParam(RepeatNode::NUM_CYCLES, num_cycles_);`
	`47`	`+ }`
	`48`	`+`
`37`	`49`	`setStatus(NodeStatus::RUNNING);`
`38`	`50`	`NodeStatus child_state = child_node_->executeTick();`
`39`	`51`
`40`	`52`	`switch (child_state)`
`41`	`53`	`{`
`42`	`54`	`case NodeStatus::SUCCESS:`
`43`	`55`	`{`
`44`		`- TryIndx_++;`
`45`		`- if (TryIndx_ >= NTries_)`
	`56`	`+ try_index_++;`
	`57`	`+ if (try_index_ >= num_cycles_)`
`46`	`58`	`{`
`47`	`59`	`setStatus(NodeStatus::SUCCESS);`
`48`	`60`	`child_node_->setStatus(NodeStatus::IDLE);`
`@@ -52,7 +64,7 @@ NodeStatus RepeatNode::tick()`
`52`	`64`
`53`	`65`	`case NodeStatus::FAILURE:`
`54`	`66`	`{`
`55`		`- TryIndx_ = 0;`
	`67`	`+ try_index_ = 0;`
`56`	`68`	`setStatus(NodeStatus::FAILURE);`
`57`	`69`	`child_node_->setStatus(NodeStatus::IDLE);`
`58`	`70`	`}`
Original file line number	Diff line number	Diff line change
`@@ -18,41 +18,53 @@ namespace BT`
`18`	`18`	`constexpr const char* RetryNode::NUM_ATTEMPTS;`
`19`	`19`
`20`	`20`	`RetryNode::RetryNode(const std::string& name, unsigned int NTries)`
`21`		`- : DecoratorNode(name, {{NUM_ATTEMPTS, std::to_string(NTries)}}), NTries_(NTries), TryIndx_(0)`
	`21`	`+ : DecoratorNode(name, {{NUM_ATTEMPTS, std::to_string(NTries)}}),`
	`22`	`+ max_attempts_(NTries),`
	`23`	`+ try_index_(0),`
	`24`	`+ refresh_parameter_(false)`
`22`	`25`	`{`
`23`	`26`	`}`
`24`	`27`
`25`	`28`	`RetryNode::RetryNode(const std::string& name, const NodeParameters& params)`
`26`		`- : DecoratorNode(name, params), NTries_(1), TryIndx_(0)`
	`29`	`+ : DecoratorNode(name, params),`
	`30`	`+ try_index_(0),`
	`31`	`+ refresh_parameter_(false)`
`27`	`32`	`{`
`28`		`- auto param = getParam<int>(NUM_ATTEMPTS);`
`29`		`- if (param)`
	`33`	`+ if( !getParam(NUM_ATTEMPTS, max_attempts_) )`
`30`	`34`	`{`
`31`		`- NTries_ = param.value();`
	`35`	`+ throw std::runtime_error("Missing parameter [num_attempts] in RetryNode");`
`32`	`36`	`}`
	`37`	`+ refresh_parameter_ = isBlackboardPattern( params.begin()->second );`
`33`	`38`	`}`
`34`	`39`
`35`	`40`	`NodeStatus RetryNode::tick()`
`36`	`41`	`{`
	`42`	`+ if( refresh_parameter_ )`
	`43`	`+ {`
	`44`	`+ // Read it at every tick. Since it points to the blackboard,`
	`45`	`+ // it may change dynamically`
	`46`	`+ getParam(NUM_ATTEMPTS, max_attempts_);`
	`47`	`+ }`
	`48`	`+`
`37`	`49`	`setStatus(NodeStatus::RUNNING);`
`38`	`50`	`NodeStatus child_state = child_node_->executeTick();`
`39`	`51`
`40`	`52`	`switch (child_state)`
`41`	`53`	`{`
`42`	`54`	`case NodeStatus::SUCCESS:`
`43`	`55`	`{`
`44`		`- TryIndx_ = 0;`
	`56`	`+ try_index_ = 0;`
`45`	`57`	`setStatus(NodeStatus::SUCCESS);`
`46`	`58`	`child_node_->setStatus(NodeStatus::IDLE);`
`47`	`59`	`}`
`48`	`60`	`break;`
`49`	`61`
`50`	`62`	`case NodeStatus::FAILURE:`
`51`	`63`	`{`
`52`		`- TryIndx_++;`
`53`		`- if (TryIndx_ >= NTries_)`
	`64`	`+ try_index_++;`
	`65`	`+ if (try_index_ >= max_attempts_)`
`54`	`66`	`{`
`55`		`- TryIndx_ = 0;`
	`67`	`+ try_index_ = 0;`
`56`	`68`	`setStatus(NodeStatus::FAILURE);`
`57`	`69`	`child_node_->setStatus(NodeStatus::IDLE);`
`58`	`70`	`}`