Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "behaviortree_cpp/decorators/repeat_node.h"
00015
00016 namespace BT
00017 {
00018 constexpr const char* RepeatNode::NUM_CYCLES;
00019
00020 RepeatNode::RepeatNode(const std::string& name, int NTries)
00021 : DecoratorNode(name, {} ),
00022 num_cycles_(NTries),
00023 try_index_(0),
00024 read_parameter_from_ports_(false)
00025 {
00026 setRegistrationID("Repeat");
00027 }
00028
00029 RepeatNode::RepeatNode(const std::string& name, const NodeConfiguration& config)
00030 : DecoratorNode(name, config),
00031 num_cycles_(0),
00032 try_index_(0),
00033 read_parameter_from_ports_(true)
00034 {
00035
00036 }
00037
00038 NodeStatus RepeatNode::tick()
00039 {
00040 if( read_parameter_from_ports_ )
00041 {
00042 if( !getInput(NUM_CYCLES, num_cycles_) )
00043 {
00044 throw RuntimeError("Missing parameter [", NUM_CYCLES, "] in RepeatNode");
00045 }
00046 }
00047
00048 setStatus(NodeStatus::RUNNING);
00049
00050 while (try_index_ < num_cycles_ || num_cycles_== -1 )
00051 {
00052 NodeStatus child_state = child_node_->executeTick();
00053
00054 switch (child_state)
00055 {
00056 case NodeStatus::SUCCESS:
00057 {
00058 try_index_++;
00059 }
00060 break;
00061
00062 case NodeStatus::FAILURE:
00063 {
00064 try_index_ = 0;
00065 return (NodeStatus::FAILURE);
00066 }
00067
00068 case NodeStatus::RUNNING:
00069 {
00070 return NodeStatus::RUNNING;
00071 }
00072
00073 default:
00074 {
00075 throw LogicError("A child node must never return IDLE");
00076 }
00077 }
00078 }
00079
00080 try_index_ = 0;
00081 return NodeStatus::SUCCESS;
00082 }
00083
00084 void RepeatNode::halt()
00085 {
00086 try_index_ = 0;
00087 DecoratorNode::halt();
00088 }
00089
00090 }