Adding NodeParameters to ParallelNode

Author: Davide Faconti

include/behaviortree_cpp/controls/parallel_node.h

@@ -21,8 +21,17 @@ namespace BT
 class ParallelNode : public ControlNode
 {
   public:
-    // Constructor
-    ParallelNode(const std::string& name, int threshold_M);
+
+    ParallelNode(const std::string& name, int threshold);
+
+    ParallelNode(const std::string& name, const NodeParameters& params);
+
+    static const NodeParameters& requiredNodeParameters()
+    {
+        static NodeParameters params = {{THRESHOLD_KEY, "1"}};
+        return params;
+    }
+
     ~ParallelNode() = default;
 
     virtual void halt() override;
@@ -31,10 +40,13 @@ class ParallelNode : public ControlNode
     void setThresholdM(unsigned int threshold_M);
 
   private:
-    unsigned int threshold_M_;
+    unsigned int threshold_;
     unsigned int success_childred_num_;
     unsigned int failure_childred_num_;
 
+    bool refresh_parameter_;
+    static constexpr const char* THRESHOLD_KEY = "threshold";
+
     virtual BT::NodeStatus tick() override;
 };
 }

include/behaviortree_cpp/tree_node.h

@@ -140,7 +140,7 @@ class TreeNode
         try
         {
             // check if it follows this ${pattern}, if it does, search inside the blackboard
-            if (bb_ && str.size() >= 4 && str[0] == '$' && str[1] == '{' && str.back() == '}')
+            if ( bb_ && isBlackboardPattern(str))
             {
                 const std::string stripped_key(&str[2], str.size() - 3);
                 bool found = bb_->get(stripped_key, destination);
@@ -182,6 +182,9 @@ class TreeNode
     const NodeParameters parameters_;
 
     Blackboard::Ptr bb_;
+
+protected:
+    static bool isBlackboardPattern(const std::string& str );
 };
 }
 

src/controls/parallel_node.cpp

@@ -13,13 +13,41 @@
 
 #include "behaviortree_cpp/controls/parallel_node.h"
 
-BT::ParallelNode::ParallelNode(const std::string& name, int threshold_M)
-  : ControlNode::ControlNode(name, NodeParameters()), threshold_M_(threshold_M)
+namespace BT
+{
+
+constexpr const char* ParallelNode::THRESHOLD_KEY;
+
+ParallelNode::ParallelNode(const std::string& name, int threshold)
+  : ControlNode::ControlNode(name, {{THRESHOLD_KEY, std::to_string(threshold)}}),
+    threshold_(threshold),
+    refresh_parameter_(false)
 {
 }
 
-BT::NodeStatus BT::ParallelNode::tick()
+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 )
+    {
+        throw std::runtime_error("Missing parameter [threshold] in ParallelNode");
+    }
+    refresh_parameter_ = isBlackboardPattern( params.begin()->second );
+  }
+
+NodeStatus ParallelNode::tick()
 {
+    if( refresh_parameter_ )
+    {
+        // Read it at every tick. Since it points to the blackboard,
+        // it may change dynamically
+        getParam(THRESHOLD_KEY, threshold_);
+    }
+
     success_childred_num_ = 0;
     failure_childred_num_ = 0;
     // Vector size initialization. children_count_ could change at runtime if you edit the tree
@@ -35,9 +63,9 @@ BT::NodeStatus BT::ParallelNode::tick()
         switch (child_status)
         {
             case NodeStatus::SUCCESS:
-                child_node->setStatus(
-                    NodeStatus::IDLE);   // the child goes in idle if it has returned success.
-                if (++success_childred_num_ == threshold_M_)
+                child_node->setStatus(NodeStatus::IDLE);
+                // the child goes in idle if it has returned success.
+                if (++success_childred_num_ == threshold_)
                 {
                     success_childred_num_ = 0;
                     failure_childred_num_ = 0;
@@ -46,9 +74,9 @@ BT::NodeStatus BT::ParallelNode::tick()
                 }
                 break;
             case NodeStatus::FAILURE:
-                child_node->setStatus(
-                    NodeStatus::IDLE);   // the child goes in idle if it has returned failure.
-                if (++failure_childred_num_ > children_count - threshold_M_)
+                child_node->setStatus(NodeStatus::IDLE);
+                // the child goes in idle if it has returned failure.
+                if (++failure_childred_num_ > children_count - threshold_)
                 {
                     success_childred_num_ = 0;
                     failure_childred_num_ = 0;
@@ -66,19 +94,21 @@ BT::NodeStatus BT::ParallelNode::tick()
     return NodeStatus::RUNNING;
 }
 
-void BT::ParallelNode::halt()
+void ParallelNode::halt()
 {
     success_childred_num_ = 0;
     failure_childred_num_ = 0;
-    BT::ControlNode::halt();
+    ControlNode::halt();
 }
 
-unsigned int BT::ParallelNode::thresholdM()
+unsigned int ParallelNode::thresholdM()
 {
-    return threshold_M_;
+    return threshold_;
 }
 
-void BT::ParallelNode::setThresholdM(unsigned int threshold_M)
+void ParallelNode::setThresholdM(unsigned int threshold_M)
 {
-    threshold_M_ = threshold_M;
+    threshold_ = threshold_M;
+}
+
 }

src/tree_node.cpp

@@ -103,6 +103,11 @@ void TreeNode::setRegistrationName(const std::string& registration_name)
     registration_name_ = registration_name;
 }
 
+bool TreeNode::isBlackboardPattern(const std::string &str)
+{
+    return str.size() >= 4 && str[0] == '$' && str[1] == '{' && str.back() == '}';
+}
+
 const std::string& TreeNode::registrationName() const
 {
     return registration_name_;