navigation_test.cpp
Go to the documentation of this file.
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  }
58  {
59  return _expected_result ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
60  }
61  void resetTickCount() { _tick_count = 0; }
62  int tickCount() const { return _tick_count;}
63 
65  {
66  std::cout << _name << ": "<< (_expected_result? "true" : "false") << std::endl;
67  _tick_count++;
68  return expectedResult();
69  }
70 
71  private:
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 SyncActionNode, public TestNode
89 {
90  public:
91  BackUpAndSpin(const std::string& name): SyncActionNode(name), TestNode(name){}
92 
93  NodeStatus tick() override
94  {
95  return tickImpl();
96  }
97 };
98 
100 {
101  public:
102  ComputePathToPose(const std::string& name): SyncActionNode(name), TestNode(name){}
103 
104  NodeStatus tick() override
105  {
106  return tickImpl();
107  }
108 };
109 
110 class FollowPath: public CoroActionNode, public TestNode
111 {
112  public:
113  FollowPath(const std::string& name): CoroActionNode(name), TestNode(name), _halted(false){}
114 
115  NodeStatus tick() override
116  {
117  _halted = false;
118  std::cout << "FollowPath::started" << std::endl;
119  auto initial_time = Now();
120 
121  // Yield for 1 second
122  while( Now() < initial_time + Milliseconds(600) )
123  {
124  setStatusRunningAndYield();
125 
126  }
127  return tickImpl();
128  }
129  void halt() override {
130  std::cout << "FollowPath::halt" << std::endl;
131  _halted = true;
133  }
134 
135  bool wasHalted() const { return _halted; }
136 
137  private:
138  bool _halted;
139 };
140 
141 //-------------------------------------
142 
143 template <typename Original, typename Casted>
144 void TryDynamicCastPtr(Original* ptr, Casted*& destination)
145 {
146  if( dynamic_cast<Casted*>(ptr) )
147  {
148  destination = dynamic_cast<Casted*>(ptr);
149  }
150 }
151 
152 /****************TESTS START HERE***************************/
153 
154 TEST(Navigationtest, MoveBaseRecovery)
155 {
156  BehaviorTreeFactory factory;
157 
158  factory.registerNodeType<IsStuck>("IsStuck");
159  factory.registerNodeType<BackUpAndSpin>("BackUpAndSpin");
160  factory.registerNodeType<ComputePathToPose>("ComputePathToPose");
161  factory.registerNodeType<FollowPath>("FollowPath");
162 
163  auto tree = buildTreeFromText(factory, xml_text);
164 
165  // Need to retrieve the node pointers with dynamic cast
166  // In a normal application you would NEVER want to do such a thing.
167  IsStuck *first_stuck_node = nullptr;
168  IsStuck *second_stuck_node = nullptr;
169  BackUpAndSpin* back_spin_node = nullptr;
170  ComputePathToPose* compute_node = nullptr;
171  FollowPath* follow_node = nullptr;
172 
173  for (auto& node: tree.nodes)
174  {
175  auto ptr = node.get();
176 
177  if( !first_stuck_node )
178  {
179  TryDynamicCastPtr(ptr, first_stuck_node);
180  }
181  else{
182  TryDynamicCastPtr(ptr, second_stuck_node);
183  }
184  TryDynamicCastPtr(ptr, back_spin_node);
185  TryDynamicCastPtr(ptr, follow_node);
186  TryDynamicCastPtr(ptr, compute_node);
187  }
188 
189  ASSERT_TRUE( first_stuck_node );
190  ASSERT_TRUE( second_stuck_node );
191  ASSERT_TRUE( back_spin_node );
192  ASSERT_TRUE( compute_node );
193  ASSERT_TRUE( follow_node );
194 
195  std::cout << "-----------------------" << std::endl;
196 
197  // First case: not stuck, everything fine.
198  NodeStatus status = NodeStatus::IDLE;
199 
200  first_stuck_node->setExpectedResult(false);
201 
202  while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
203  {
204  status = tree.root_node->executeTick();
205  std::this_thread::sleep_for(Milliseconds(100));
206  }
207 
208  // SUCCESS expected
209  ASSERT_EQ( status, NodeStatus::SUCCESS );
210  // IsStuck on the left branch must run several times
211  ASSERT_GE( first_stuck_node->tickCount(), 6);
212  // Never take the right branch (recovery)
213  ASSERT_EQ( second_stuck_node->tickCount(), 0 );
214  ASSERT_EQ( back_spin_node->tickCount(), 0 );
215 
216  ASSERT_EQ( compute_node->tickCount(), 1 );
217  ASSERT_EQ( follow_node->tickCount(), 1 );
218  ASSERT_FALSE( follow_node->wasHalted() );
219 
220  std::cout << "-----------------------" << std::endl;
221 
222  // Second case: get stuck after a while
223 
224  // Initialize evrything first
225  first_stuck_node->resetTickCount();
226  second_stuck_node->resetTickCount();
227  compute_node->resetTickCount();
228  follow_node->resetTickCount();
229  back_spin_node->resetTickCount();
230  status = NodeStatus::IDLE;
231  int cycle = 0;
232 
233  while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
234  {
235  // At the fifth cycle get stucked
236  if( ++cycle == 2 )
237  {
238  first_stuck_node->setExpectedResult(true);
239  second_stuck_node->setExpectedResult(true);
240  }
241  status = tree.root_node->executeTick();
242  std::this_thread::sleep_for(Milliseconds(100));
243  }
244 
245  // SUCCESS expected
246  ASSERT_EQ( status, NodeStatus::SUCCESS );
247 
248  // First IsStuck must run several times
249  ASSERT_GE( first_stuck_node->tickCount(), 2);
250  // Second IsStuck probably only once
251  ASSERT_EQ( second_stuck_node->tickCount(), 1 );
252  ASSERT_EQ( back_spin_node->tickCount(), 1 );
253 
254  // compute done once and follow started but halted
255  ASSERT_EQ( compute_node->tickCount(), 1 );
256 
257  ASSERT_EQ( follow_node->tickCount(), 0 ); // started but never completed
258  ASSERT_TRUE( follow_node->wasHalted() );
259 
260  ASSERT_EQ( compute_node->status(), NodeStatus::IDLE );
261  ASSERT_EQ( follow_node->status(), NodeStatus::IDLE );
262  ASSERT_EQ( back_spin_node->status(), NodeStatus::IDLE );
263 
264 
265  std::cout << "-----------------------" << std::endl;
266 
267  // Third case: execute again
268 
269  // Initialize everything first
270  first_stuck_node->resetTickCount();
271  second_stuck_node->resetTickCount();
272  compute_node->resetTickCount();
273  follow_node->resetTickCount();
274  back_spin_node->resetTickCount();
275  status = NodeStatus::IDLE;
276  first_stuck_node->setExpectedResult(false);
277  second_stuck_node->setExpectedResult(false);
278 
279  while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
280  {
281  status = tree.root_node->executeTick();
282  std::this_thread::sleep_for(Milliseconds(100));
283  }
284 
285  // SUCCESS expected
286  ASSERT_EQ( status, NodeStatus::SUCCESS );
287 
288  ASSERT_GE( first_stuck_node->tickCount(), 6);
289  ASSERT_EQ( second_stuck_node->tickCount(), 0 );
290  ASSERT_EQ( back_spin_node->tickCount(), 0 );
291 
292  ASSERT_EQ( compute_node->status(), NodeStatus::IDLE );
293  ASSERT_EQ( follow_node->status(), NodeStatus::IDLE );
294  ASSERT_EQ( back_spin_node->status(), NodeStatus::IDLE );
295  ASSERT_FALSE( follow_node->wasHalted() );
296 
297 }
298 
299 
NodeStatus tick() override
Method to be implemented by the user.
BackUpAndSpin(const std::string &name)
void registerNodeType(const std::string &ID)
Definition: bt_factory.h:98
bool _expected_result
virtual NodeStatus executeTick() overridefinal
The method that will be executed to invoke tick(); and setStatus();.
ComputePathToPose(const std::string &name)
NodeStatus tick() override
Method to be implemented by the user.
void halt() override
NodeStatus tick() override
Method to be implemented by the user.
NodeStatus tickImpl()
The CoroActionNode class is an ideal candidate for asynchronous actions which need to communicate wit...
Definition: action_node.h:158
void setExpectedResult(bool will_succeed)
FollowPath(const std::string &name)
The SyncActionNode is an helper derived class that explicitly forbids the status RUNNING and doesn&#39;t ...
Definition: action_node.h:50
void resetTickCount()
std::string _name
void halt() override
The method used to interrupt the execution of a RUNNING node.
bool wasHalted() const
NodeStatus tick() override
Method to be implemented by the user.
NodeStatus status() const
Definition: tree_node.cpp:75
TestNode(const std::string &name)
NodeStatus
Definition: basic_types.h:28
NodeStatus expectedResult() const
int tickCount() const
IsStuck(const std::string &name)
Tree buildTreeFromText(const BehaviorTreeFactory &factory, const std::string &text, const Blackboard::Ptr &blackboard=Blackboard::Ptr())


behaviortree_cpp
Author(s): Michele Colledanchise, Davide Faconti
autogenerated on Sun Feb 3 2019 03:14:32