Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit 3383e5a

Browse files
author
Davide Faconti
committed
Adding example/test of navigation and recovery behavior. Related to issue BehaviorTree#36 
1 parent cd8ef71 commit 3383e5a

File tree

2 files changed

+269
-0
lines changed

2 files changed

+269
-0
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -167,6 +167,7 @@ set(BT_TESTS
167167
gtest/gtest_factory.cpp
168168
gtest/gtest_decorator.cpp
169169
gtest/gtest_blackboard.cpp
170+
gtest/navigation_test.cpp
170171
)
171172

172173
if(ament_cmake_FOUND AND BUILD_TESTING)

‎gtest/navigation_test.cpp

Lines changed: 268 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,268 @@
1+
#include "behaviortree_cpp/xml_parsing.h"
2+
#include "behaviortree_cpp/blackboard/blackboard_local.h"
3+
#include <gtest/gtest.h>
4+
5+
using namespace BT;
6+
7+
// clang-format off
8+
const std::string xml_text = R"(
9+
10+
<root main_tree_to_execute="BehaviorTree">
11+
<BehaviorTree ID="BehaviorTree">
12+
<FallbackStar name="root">
13+
<Sequence name="navigation_subtree">
14+
<Inverter>
15+
<Condition ID="IsStuck"/>
16+
</Inverter>
17+
<SequenceStar name="navigate">
18+
<Action ID="ComputePathToPose"/>
19+
<Action ID="FollowPath"/>
20+
</SequenceStar>
21+
</Sequence>
22+
<SequenceStar name="stuck_recovery">
23+
<Condition ID="IsStuck"/>
24+
<Action ID="BackUpAndSpin"/>
25+
</SequenceStar>
26+
</FallbackStar>
27+
</BehaviorTree>
28+
</root>
29+
)";
30+
31+
// clang-format on
32+
33+
using Milliseconds = std::chrono::milliseconds;
34+
35+
inline std::chrono::high_resolution_clock::time_point Now()
36+
{
37+
return std::chrono::high_resolution_clock::now();
38+
}
39+
40+
41+
42+
//--------------------------------------------
43+
44+
class TestNode
45+
{
46+
public:
47+
TestNode(const std::string& name):
48+
_expected_result(true),
49+
_tick_count(0),
50+
_name(name)
51+
{ }
52+
53+
void setExpectedResult(bool will_succeed)
54+
{
55+
_expected_result = will_succeed;
56+
}
57+
NodeStatus expectedResult() const
58+
{
59+
return _expected_result ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
60+
}
61+
void resetTickCount() { _tick_count = 0; }
62+
int tickCount() const { return _tick_count;}
63+
64+
NodeStatus tickImpl()
65+
{
66+
std::cout << _name << "::tick completed" << std::endl;
67+
_tick_count++;
68+
return expectedResult();
69+
}
70+
71+
private:
72+
bool _expected_result;
73+
int _tick_count;
74+
std::string _name;
75+
};
76+
77+
class IsStuck: public ConditionNode, public TestNode
78+
{
79+
public:
80+
IsStuck(const std::string& name): ConditionNode(name), TestNode(name) {}
81+
82+
NodeStatus tick() override
83+
{
84+
return tickImpl();
85+
}
86+
};
87+
88+
class BackUpAndSpin: public ActionNodeBase, public TestNode
89+
{
90+
public:
91+
BackUpAndSpin(const std::string& name): ActionNodeBase(name), TestNode(name){}
92+
93+
NodeStatus tick() override
94+
{
95+
return tickImpl();
96+
}
97+
void halt() override {
98+
std::cout << "BackUpAndSpin::halt" << std::endl;
99+
}
100+
};
101+
102+
class ComputePathToPose: public ActionNodeBase, public TestNode
103+
{
104+
public:
105+
ComputePathToPose(const std::string& name): ActionNodeBase(name), TestNode(name){}
106+
107+
NodeStatus tick() override
108+
{
109+
return tickImpl();
110+
}
111+
void halt() override {
112+
std::cout << "ComputePathToPose::halt" << std::endl;
113+
}
114+
};
115+
116+
class FollowPath: public CoroActionNode, public TestNode
117+
{
118+
public:
119+
FollowPath(const std::string& name): CoroActionNode(name), TestNode(name), _halted(false){}
120+
121+
NodeStatus tick() override
122+
{
123+
_halted = false;
124+
std::cout << "FollowPath::started" << std::endl;
125+
auto initial_time = Now();
126+
127+
// Yield for 1 second
128+
while( Now() < initial_time + Milliseconds(1000) )
129+
{
130+
setStatusRunningAndYield();
131+
132+
}
133+
return tickImpl();
134+
}
135+
void halt() override {
136+
std::cout << "FollowPath::halt" << std::endl;
137+
setStatus( NodeStatus::FAILURE );
138+
_halted = true;
139+
CoroActionNode::halt();
140+
}
141+
142+
bool wasHalted() const { return _halted; }
143+
144+
private:
145+
bool _halted;
146+
};
147+
148+
//-------------------------------------
149+
150+
/****************TESTS START HERE***************************/
151+
152+
TEST(Navigationtest, MoveBaseReocvery)
153+
{
154+
BehaviorTreeFactory factory;
155+
156+
factory.registerNodeType<IsStuck>("IsStuck");
157+
factory.registerNodeType<BackUpAndSpin>("BackUpAndSpin");
158+
factory.registerNodeType<ComputePathToPose>("ComputePathToPose");
159+
factory.registerNodeType<FollowPath>("FollowPath");
160+
161+
auto tree = buildTreeFromText(factory, xml_text);
162+
163+
IsStuck *first_stuck_node = nullptr;
164+
IsStuck *second_stuck_node = nullptr;
165+
BackUpAndSpin* back_spin_node = nullptr;
166+
ComputePathToPose* compute_node = nullptr;
167+
FollowPath* follow_node = nullptr;
168+
169+
for (auto& node: tree.nodes)
170+
{
171+
auto ptr = node.get();
172+
if( dynamic_cast<IsStuck*>(ptr) )
173+
{
174+
if( !first_stuck_node )
175+
{
176+
first_stuck_node = dynamic_cast<IsStuck*>(ptr);
177+
}
178+
else{
179+
second_stuck_node = dynamic_cast<IsStuck*>(ptr);
180+
}
181+
}
182+
else if( dynamic_cast<BackUpAndSpin*>(ptr) ){
183+
back_spin_node = dynamic_cast<BackUpAndSpin*>(ptr);
184+
}
185+
else if( dynamic_cast<ComputePathToPose*>(ptr) ){
186+
compute_node = dynamic_cast<ComputePathToPose*>(ptr);
187+
}
188+
else if( dynamic_cast<FollowPath*>(ptr) ){
189+
follow_node = dynamic_cast<FollowPath*>(ptr);
190+
}
191+
}
192+
193+
ASSERT_TRUE( first_stuck_node );
194+
ASSERT_TRUE( second_stuck_node );
195+
ASSERT_TRUE( back_spin_node );
196+
ASSERT_TRUE( compute_node );
197+
ASSERT_TRUE( follow_node );
198+
199+
NodeStatus status = NodeStatus::IDLE;
200+
201+
auto deadline = Now() + Milliseconds(100);
202+
203+
first_stuck_node->setExpectedResult(false);
204+
205+
std::cout << "-----------------------" << std::endl;
206+
// First case: not stuck, everything fine.
207+
208+
while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
209+
{
210+
status = tree.root_node->executeTick();
211+
std::this_thread::sleep_until(deadline);
212+
deadline = Now() + Milliseconds(100);
213+
}
214+
215+
// SUCCESS expected
216+
ASSERT_EQ( status, NodeStatus::SUCCESS );
217+
// IsStuck on the left branch must run several times
218+
ASSERT_GE( first_stuck_node->tickCount(), 9);
219+
// Never take the right branch (recovery)
220+
ASSERT_EQ( second_stuck_node->tickCount(), 0 );
221+
ASSERT_EQ( back_spin_node->tickCount(), 0 );
222+
223+
ASSERT_EQ( compute_node->tickCount(), 1 );
224+
ASSERT_EQ( follow_node->tickCount(), 1 );
225+
ASSERT_FALSE( follow_node->wasHalted() );
226+
227+
std::cout << "-----------------------" << std::endl;
228+
first_stuck_node->resetTickCount();
229+
second_stuck_node->resetTickCount();
230+
compute_node->resetTickCount();
231+
follow_node->resetTickCount();
232+
back_spin_node->resetTickCount();
233+
234+
status = NodeStatus::IDLE;
235+
236+
int cycle = 0;
237+
deadline = Now() + Milliseconds(100);
238+
while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
239+
{
240+
if( cycle++ == 5 )
241+
{
242+
first_stuck_node->setExpectedResult(true);
243+
second_stuck_node->setExpectedResult(true);
244+
}
245+
status = tree.root_node->executeTick();
246+
std::this_thread::sleep_until(deadline);
247+
deadline = Now() + Milliseconds(100);
248+
}
249+
250+
// SUCCESS expected
251+
ASSERT_EQ( status, NodeStatus::SUCCESS );
252+
253+
// First IsStuck must run several times
254+
ASSERT_GE( first_stuck_node->tickCount(), 5);
255+
// Second IsStuck probably only once
256+
ASSERT_EQ( second_stuck_node->tickCount(), 1 );
257+
ASSERT_EQ( back_spin_node->tickCount(), 1 );
258+
259+
// compute done once and follow started but halted
260+
ASSERT_EQ( compute_node->tickCount(), 1 );
261+
262+
ASSERT_EQ( follow_node->tickCount(), 0 ); // started but never completed
263+
ASSERT_TRUE( follow_node->wasHalted() );
264+
ASSERT_EQ( follow_node->status(), NodeStatus::FAILURE );
265+
266+
}
267+
268+

0 commit comments

Comments
 (0)