Skip to content

Commit 97a434d

Browse files
facontidavidefacontidavide
authored
issue #755 : add backchaining test and change reactive nodes checks (#770)
1 parent 97c3f6a commit 97a434d

File tree

5 files changed

+191
-7
lines changed

5 files changed

+191
-7
lines changed

‎conanfile.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
[requires]
2-
gtest/1.12.1
2+
gtest/1.14.0
33
zeromq/4.3.4
44
sqlite3/3.40.1
55

‎src/controls/reactive_fallback.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
namespace BT
1616
{
1717

18-
bool ReactiveFallback::throw_if_multiple_running = true;
18+
bool ReactiveFallback::throw_if_multiple_running = false;
1919

2020
void ReactiveFallback::EnableException(bool enable)
2121
{

‎src/controls/reactive_sequence.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
namespace BT
1616
{
1717

18-
bool ReactiveSequence::throw_if_multiple_running = true;
18+
bool ReactiveSequence::throw_if_multiple_running = false;
1919

2020
void ReactiveSequence::EnableException(bool enable)
2121
{

‎tests/CMakeLists.txt

+4-4
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ set(BT_TESTS
1919
gtest_match.cpp
2020
gtest_json.cpp
2121
gtest_reactive.cpp
22+
gtest_reactive_backchaining.cpp
2223
gtest_sequence.cpp
2324
gtest_skipping.cpp
2425
gtest_substitution.cpp
@@ -55,12 +56,11 @@ elseif(catkin_FOUND AND CATKIN_ENABLE_TESTING)
5556

5657
else()
5758

58-
find_package(GTest)
59+
find_package(GTest REQUIRED)
5960
enable_testing()
6061

61-
add_executable(${BTCPP_LIBRARY}_test ${BT_TESTS}
62-
gtest_enums.cpp
63-
gtest_any.cpp)
62+
add_executable(${BTCPP_LIBRARY}_test ${BT_TESTS})
63+
6464
target_link_libraries(${PROJECT_NAME}_test
6565
${TEST_DEPENDECIES}
6666
Threads::Threads

‎tests/gtest_reactive_backchaining.cpp

+184
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,184 @@
1+
#include <gtest/gtest.h>
2+
#include "behaviortree_cpp/loggers/bt_observer.h"
3+
#include "behaviortree_cpp/bt_factory.h"
4+
5+
namespace BT::test
6+
{
7+
8+
class SimpleCondition : public BT::ConditionNode
9+
{
10+
private:
11+
std::string port_name_;
12+
public:
13+
SimpleCondition(const std::string& name, const BT::NodeConfig& config,
14+
std::string port_name) :
15+
BT::ConditionNode(name, config),
16+
port_name_(port_name)
17+
{}
18+
static BT::PortsList providedPorts() {
19+
return {};
20+
}
21+
BT::NodeStatus tick() override
22+
{
23+
auto val = config().blackboard->get<bool>(port_name_);
24+
return (val) ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
25+
}
26+
};
27+
28+
//--------------------------
29+
class AsyncTestAction : public BT::StatefulActionNode
30+
{
31+
int counter_ = 0;
32+
std::string port_name_;
33+
public:
34+
AsyncTestAction(const std::string& name, const BT::NodeConfig& config,
35+
std::string port_name) :
36+
BT::StatefulActionNode(name, config),
37+
port_name_(port_name)
38+
{}
39+
40+
static BT::PortsList providedPorts(){ return {}; }
41+
42+
NodeStatus onStart() override {
43+
counter_ = 0;
44+
return NodeStatus::RUNNING;
45+
}
46+
47+
NodeStatus onRunning() override {
48+
if(++counter_ == 2) {
49+
config().blackboard->set<bool>(port_name_, true);
50+
return NodeStatus::SUCCESS;
51+
}
52+
return NodeStatus::RUNNING;
53+
}
54+
void onHalted() override {}
55+
};
56+
//--------------------------
57+
58+
TEST(ReactiveBackchaining, EnsureWarm)
59+
{
60+
// This test shows the basic structure of a PPA: a fallback
61+
// of a postcondition and an action to make that
62+
// postcondition true.
63+
static const char* xml_text = R"(
64+
<root BTCPP_format="4">
65+
<BehaviorTree ID="EnsureWarm">
66+
<ReactiveFallback>
67+
<IsWarm name="warm"/>
68+
<ReactiveSequence>
69+
<IsHoldingJacket name="jacket" />
70+
<WearJacket name="wear" />
71+
</ReactiveSequence>
72+
</ReactiveFallback>
73+
</BehaviorTree>
74+
</root>
75+
)";
76+
77+
// The final condition of the PPA; the thing that make_warm achieves.
78+
// For this example, we're only warm after WearJacket returns success.
79+
BehaviorTreeFactory factory;
80+
factory.registerNodeType<SimpleCondition>("IsWarm", "is_warm");
81+
factory.registerNodeType<SimpleCondition>("IsHoldingJacket", "holding_jacket");
82+
factory.registerNodeType<AsyncTestAction>("WearJacket", "is_warm");
83+
84+
Tree tree = factory.createTreeFromText(xml_text);
85+
BT::TreeObserver observer(tree);
86+
87+
auto& blackboard = tree.subtrees.front()->blackboard;
88+
blackboard->set("is_warm", false);
89+
blackboard->set("holding_jacket", true);
90+
91+
// first tick: not warm, have a jacket: start wearing it
92+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
93+
EXPECT_FALSE(blackboard->get<bool>("is_warm"));
94+
95+
// second tick: not warm (still wearing)
96+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
97+
EXPECT_FALSE(blackboard->get<bool>("is_warm"));
98+
99+
// third tick: warm (wearing succeeded)
100+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::SUCCESS);
101+
EXPECT_TRUE(blackboard->get<bool>("is_warm"));
102+
103+
// fourth tick: still warm (just the condition ticked)
104+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::SUCCESS);
105+
106+
EXPECT_EQ(observer.getStatistics("warm").failure_count, 3);
107+
EXPECT_EQ(observer.getStatistics("warm").success_count, 1);
108+
109+
EXPECT_EQ(observer.getStatistics("jacket").transitions_count, 3);
110+
EXPECT_EQ(observer.getStatistics("jacket").success_count, 3);
111+
112+
EXPECT_EQ(observer.getStatistics("wear").success_count, 1);
113+
}
114+
115+
116+
TEST(ReactiveBackchaining, EnsureWarmWithEnsureHoldingHacket)
117+
{
118+
// This test backchains on HoldingHacket => EnsureHoldingHacket to iteratively add reactivity and functionality to the tree.
119+
// The general structure of the PPA remains the same.
120+
static const char* xml_text = R"(
121+
<root BTCPP_format="4">
122+
<BehaviorTree ID="EnsureWarm">
123+
<ReactiveFallback>
124+
<IsWarm />
125+
<ReactiveSequence>
126+
<SubTree ID="EnsureHoldingJacket" />
127+
<WearJacket />
128+
</ReactiveSequence>
129+
</ReactiveFallback>
130+
</BehaviorTree>
131+
132+
<BehaviorTree ID="EnsureHoldingJacket">
133+
<ReactiveFallback>
134+
<IsHoldingJacket />
135+
<ReactiveSequence>
136+
<IsNearCloset />
137+
<GrabJacket />
138+
</ReactiveSequence>
139+
</ReactiveFallback>
140+
</BehaviorTree>
141+
</root>
142+
)";
143+
144+
BehaviorTreeFactory factory;
145+
factory.registerNodeType<SimpleCondition>("IsWarm", "is_warm");
146+
factory.registerNodeType<SimpleCondition>("IsHoldingJacket", "holding_jacket");
147+
factory.registerNodeType<SimpleCondition>("IsNearCloset", "near_closet");
148+
factory.registerNodeType<AsyncTestAction>("WearJacket", "is_warm");
149+
factory.registerNodeType<AsyncTestAction>("GrabJacket", "holding_jacket");
150+
151+
factory.registerBehaviorTreeFromText(xml_text);
152+
Tree tree = factory.createTree("EnsureWarm");
153+
BT::TreeObserver observer(tree);
154+
155+
tree.subtrees[0]->blackboard->set("is_warm", false);
156+
tree.subtrees[1]->blackboard->set("holding_jacket", false);
157+
tree.subtrees[1]->blackboard->set("near_closet", true);
158+
159+
// first tick: not warm, no jacket, start GrabJacket
160+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
161+
EXPECT_FALSE(tree.subtrees[0]->blackboard->get<bool>("is_warm"));
162+
EXPECT_FALSE(tree.subtrees[1]->blackboard->get<bool>("holding_jacket"));
163+
EXPECT_TRUE(tree.subtrees[1]->blackboard->get<bool>("near_closet"));
164+
165+
// second tick: still GrabJacket
166+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
167+
168+
// third tick: GrabJacket succeeded, start wearing
169+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
170+
EXPECT_FALSE(tree.subtrees[0]->blackboard->get<bool>("is_warm"));
171+
EXPECT_TRUE(tree.subtrees[1]->blackboard->get<bool>("holding_jacket"));
172+
173+
// fourth tick: still WearingJacket
174+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
175+
176+
// fifth tick: warm (WearingJacket succeeded)
177+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::SUCCESS);
178+
EXPECT_TRUE(tree.subtrees[0]->blackboard->get<bool>("is_warm"));
179+
180+
// sixr tick: still warm (just the condition ticked)
181+
EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::SUCCESS);
182+
}
183+
184+
}

0 commit comments

Comments
 (0)