Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

threshold child count dynamically in parallel control node #363

Merged
merged 1 commit into from
Aug 17, 2022
Merged

threshold child count dynamically in parallel control node #363

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions include/behaviortree_cpp_v3/controls/parallel_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,12 +41,12 @@ class ParallelNode : public ControlNode

unsigned int thresholdM();
unsigned int thresholdFM();
void setThresholdM(unsigned int threshold_M);
void setThresholdFM(unsigned int threshold_M);
void setThresholdM(int threshold_M);
void setThresholdFM(int threshold_M);

private:
unsigned int success_threshold_;
unsigned int failure_threshold_;
int success_threshold_;
int failure_threshold_;

std::set<int> skip_list_;

Expand Down
25 changes: 16 additions & 9 deletions src/controls/parallel_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,9 @@
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <algorithm>
#include <cstddef>

#include "behaviortree_cpp_v3/controls/parallel_node.h"

namespace BT
Expand Down Expand Up @@ -58,12 +61,12 @@ NodeStatus ParallelNode::tick()

const size_t children_count = children_nodes_.size();

if( children_count < success_threshold_)
if( children_count < thresholdM())
{
throw LogicError("Number of children is less than threshold. Can never succeed.");
}

if( children_count < failure_threshold_)
if( children_count < thresholdFM())
{
throw LogicError("Number of children is less than threshold. Can never fail.");
}
Expand Down Expand Up @@ -94,7 +97,7 @@ NodeStatus ParallelNode::tick()
}
success_childred_num++;

if (success_childred_num == success_threshold_)
if (success_childred_num == thresholdM())
{
skip_list_.clear();
haltChildren();
Expand All @@ -112,8 +115,8 @@ NodeStatus ParallelNode::tick()

// It fails if it is not possible to succeed anymore or if
// number of failures are equal to failure_threshold_
if ((failure_childred_num > children_count - success_threshold_)
|| (failure_childred_num == failure_threshold_))
if ((failure_childred_num > children_count - thresholdM())
|| (failure_childred_num == thresholdFM()))
{
skip_list_.clear();
haltChildren();
Expand Down Expand Up @@ -144,20 +147,24 @@ void ParallelNode::halt()

unsigned int ParallelNode::thresholdM()
{
return success_threshold_;
return success_threshold_ < 0
? std::max(children_nodes_.size() + success_threshold_ + 1, static_cast<std::size_t>(0))
: success_threshold_;
}

unsigned int ParallelNode::thresholdFM()
{
return failure_threshold_;
return failure_threshold_ < 0
? std::max(children_nodes_.size() + failure_threshold_ + 1, static_cast<std::size_t>(0))
: failure_threshold_;
}

void ParallelNode::setThresholdM(unsigned int threshold_M)
void ParallelNode::setThresholdM(int threshold_M)
{
success_threshold_ = threshold_M;
}

void ParallelNode::setThresholdFM(unsigned int threshold_M)
void ParallelNode::setThresholdFM(int threshold_M)
{
failure_threshold_ = threshold_M;
}
Expand Down
80 changes: 80 additions & 0 deletions tests/gtest_parallel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,85 @@ TEST_F(SimpleParallelTest, Threshold_3)
ASSERT_EQ(NodeStatus::SUCCESS, state);
}

TEST_F(SimpleParallelTest, Threshold_neg2)
{
root.setThresholdM(-2);
action_1.setTime( milliseconds(100) );
action_2.setTime( milliseconds(500) ); // this takes a lot of time

BT::NodeStatus state = root.executeTick();
// first tick, zero wait
ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
ASSERT_EQ(NodeStatus::RUNNING, state);

std::this_thread::sleep_for( milliseconds(150) );
state = root.executeTick();
// second tick: action1 should be completed, but not action2
// nevertheless it is sufficient because threshold is 3
ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
ASSERT_EQ(NodeStatus::IDLE, action_1.status());
ASSERT_EQ(NodeStatus::IDLE, action_2.status());
ASSERT_EQ(NodeStatus::SUCCESS, state);
}


TEST_F(SimpleParallelTest, Threshold_neg1)
{
root.setThresholdM(-1);
action_1.setTime( milliseconds(100) );
action_2.setTime( milliseconds(500) ); // this takes a lot of time

BT::NodeStatus state = root.executeTick();
// first tick, zero wait
ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
ASSERT_EQ(NodeStatus::RUNNING, state);

std::this_thread::sleep_for( milliseconds(150) );
state = root.executeTick();
// second tick: action1 should be completed, but not action2
ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
ASSERT_EQ(NodeStatus::RUNNING, state);

std::this_thread::sleep_for( milliseconds(650) );
state = root.executeTick();
// third tick: all actions completed
ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
ASSERT_EQ(NodeStatus::IDLE, action_1.status());
ASSERT_EQ(NodeStatus::IDLE, action_2.status());
ASSERT_EQ(NodeStatus::SUCCESS, state);
}


TEST_F(SimpleParallelTest, Threshold_thresholdFneg1)
{
root.setThresholdM(1);
root.setThresholdFM(-1);
action_1.setTime( milliseconds(100) );
action_1.setExpectedResult(NodeStatus::FAILURE);
condition_1.setExpectedResult(NodeStatus::FAILURE);
action_2.setTime( milliseconds(200) );
condition_2.setExpectedResult(NodeStatus::FAILURE);
action_2.setExpectedResult(NodeStatus::FAILURE);

BT::NodeStatus state = root.executeTick();
ASSERT_EQ(NodeStatus::RUNNING, state);

std::this_thread::sleep_for(milliseconds(250));
state = root.executeTick();
ASSERT_EQ(NodeStatus::FAILURE, state);
}

TEST_F(SimpleParallelTest, Threshold_2)
{
root.setThresholdM(2);
Expand Down Expand Up @@ -271,6 +350,7 @@ TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
ASSERT_EQ(NodeStatus::SUCCESS, state);
}


TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
{
condition_R.setExpectedResult(NodeStatus::FAILURE);
Expand Down