Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "behaviortree_cpp/controls/parallel_node.h"
00015
00016 namespace BT
00017 {
00018
00019 constexpr const char* ParallelNode::THRESHOLD_KEY;
00020
00021 ParallelNode::ParallelNode(const std::string& name, unsigned threshold)
00022 : ControlNode::ControlNode(name, {} ),
00023 threshold_(threshold),
00024 read_parameter_from_ports_(false)
00025 {
00026 setRegistrationID("Parallel");
00027 }
00028
00029 ParallelNode::ParallelNode(const std::string &name,
00030 const NodeConfiguration& config)
00031 : ControlNode::ControlNode(name, config),
00032 threshold_(0),
00033 read_parameter_from_ports_(true)
00034 {
00035 }
00036
00037 NodeStatus ParallelNode::tick()
00038 {
00039 if(read_parameter_from_ports_)
00040 {
00041 if( !getInput(THRESHOLD_KEY, threshold_) )
00042 {
00043 throw RuntimeError("Missing parameter [", THRESHOLD_KEY, "] in ParallelNode");
00044 }
00045 }
00046
00047 size_t success_childred_num = 0;
00048 size_t failure_childred_num = 0;
00049
00050 const size_t children_count = children_nodes_.size();
00051
00052 if( children_count < threshold_)
00053 {
00054 throw LogicError("Number of children is less than threshold. Can never suceed.");
00055 }
00056
00057
00058 for (unsigned int i = 0; i < children_count; i++)
00059 {
00060 TreeNode* child_node = children_nodes_[i];
00061
00062 bool in_skip_list = (skip_list_.count(i) != 0);
00063
00064 NodeStatus child_status;
00065 if( in_skip_list )
00066 {
00067 child_status = child_node->status();
00068 }
00069 else {
00070 child_status = child_node->executeTick();
00071 }
00072
00073 switch (child_status)
00074 {
00075 case NodeStatus::SUCCESS:
00076 {
00077 if( !in_skip_list )
00078 {
00079 skip_list_.insert(i);
00080 }
00081 success_childred_num++;
00082
00083 if (success_childred_num == threshold_)
00084 {
00085 skip_list_.clear();
00086 haltChildren(0);
00087 return NodeStatus::SUCCESS;
00088 }
00089 } break;
00090
00091 case NodeStatus::FAILURE:
00092 {
00093 if( !in_skip_list )
00094 {
00095 skip_list_.insert(i);
00096 }
00097 failure_childred_num++;
00098
00099 if (failure_childred_num > children_count - threshold_)
00100 {
00101 skip_list_.clear();
00102 haltChildren(0);
00103 return NodeStatus::FAILURE;
00104 }
00105 } break;
00106
00107 case NodeStatus::RUNNING:
00108 {
00109
00110 } break;
00111
00112 default:
00113 {
00114 throw LogicError("A child node must never return IDLE");
00115 }
00116 }
00117 }
00118
00119 return NodeStatus::RUNNING;
00120 }
00121
00122 void ParallelNode::halt()
00123 {
00124 skip_list_.clear();
00125 ControlNode::halt();
00126 }
00127
00128 unsigned int ParallelNode::thresholdM()
00129 {
00130 return threshold_;
00131 }
00132
00133 void ParallelNode::setThresholdM(unsigned int threshold_M)
00134 {
00135 threshold_ = threshold_M;
00136 }
00137
00138 }