00001 #include "behaviortree_cpp/xml_parsing.h"
00002 #include "behaviortree_cpp/blackboard.h"
00003 #include <gtest/gtest.h>
00004
00005 using namespace BT;
00006
00007
00008 static const char* xml_text = R"(
00009
00010 <root main_tree_to_execute="BehaviorTree">
00011 <BehaviorTree ID="BehaviorTree">
00012 <Fallback name="root">
00013
00014 <ReactiveSequence name="navigation_subtree">
00015 <Inverter>
00016 <Condition ID="IsStuck"/>
00017 </Inverter>
00018 <SequenceStar name="navigate">
00019 <Action ID="ComputePathToPose"/>
00020 <Action ID="FollowPath"/>
00021 </SequenceStar>
00022 </ReactiveSequence>
00023
00024 <SequenceStar name="stuck_recovery">
00025 <Condition ID="IsStuck"/>
00026 <Action ID="BackUpAndSpin"/>
00027 </SequenceStar>
00028
00029 </Fallback>
00030 </BehaviorTree>
00031 </root>
00032 )";
00033
00034
00035
00036 using Milliseconds = std::chrono::milliseconds;
00037
00038 inline std::chrono::high_resolution_clock::time_point Now()
00039 {
00040 return std::chrono::high_resolution_clock::now();
00041 }
00042
00043
00044
00045
00046
00047 class TestNode
00048 {
00049 public:
00050 TestNode(const std::string& name):
00051 _expected_result(true),
00052 _tick_count(0),
00053 _name(name)
00054 { }
00055
00056 void setExpectedResult(bool will_succeed)
00057 {
00058 _expected_result = will_succeed;
00059 }
00060 NodeStatus expectedResult() const
00061 {
00062 return _expected_result ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
00063 }
00064 void resetTickCount() { _tick_count = 0; }
00065 int tickCount() const { return _tick_count;}
00066
00067 NodeStatus tickImpl()
00068 {
00069 std::cout << _name << ": "<< (_expected_result? "true" : "false") << std::endl;
00070 _tick_count++;
00071 return expectedResult();
00072 }
00073
00074 private:
00075 bool _expected_result;
00076 int _tick_count;
00077 std::string _name;
00078 };
00079
00080 class IsStuck: public ConditionNode, public TestNode
00081 {
00082 public:
00083 IsStuck(const std::string& name): ConditionNode(name, {}), TestNode(name) {}
00084
00085 NodeStatus tick() override
00086 {
00087 return tickImpl();
00088 }
00089 };
00090
00091 class BackUpAndSpin: public SyncActionNode, public TestNode
00092 {
00093 public:
00094 BackUpAndSpin(const std::string& name): SyncActionNode(name, {}), TestNode(name){}
00095
00096 NodeStatus tick() override
00097 {
00098 return tickImpl();
00099 }
00100 };
00101
00102 class ComputePathToPose: public SyncActionNode, public TestNode
00103 {
00104 public:
00105 ComputePathToPose(const std::string& name): SyncActionNode(name, {}), TestNode(name){}
00106
00107 NodeStatus tick() override
00108 {
00109 return tickImpl();
00110 }
00111 };
00112
00113 class FollowPath: public CoroActionNode, public TestNode
00114 {
00115 public:
00116 FollowPath(const std::string& name): CoroActionNode(name, {}), TestNode(name), _halted(false){}
00117
00118 NodeStatus tick() override
00119 {
00120 _halted = false;
00121 std::cout << "FollowPath::started" << std::endl;
00122 auto initial_time = Now();
00123
00124
00125 while( Now() < initial_time + Milliseconds(600) )
00126 {
00127 setStatusRunningAndYield();
00128 }
00129 return tickImpl();
00130 }
00131 void halt() override {
00132 std::cout << "FollowPath::halt" << std::endl;
00133 _halted = true;
00134 CoroActionNode::halt();
00135 }
00136
00137 bool wasHalted() const { return _halted; }
00138
00139 private:
00140 bool _halted;
00141 };
00142
00143
00144
00145 template <typename Original, typename Casted>
00146 void TryDynamicCastPtr(Original* ptr, Casted*& destination)
00147 {
00148 if( dynamic_cast<Casted*>(ptr) )
00149 {
00150 destination = dynamic_cast<Casted*>(ptr);
00151 }
00152 }
00153
00154
00155
00156 TEST(Navigationtest, MoveBaseRecovery)
00157 {
00158 BehaviorTreeFactory factory;
00159
00160 factory.registerNodeType<IsStuck>("IsStuck");
00161 factory.registerNodeType<BackUpAndSpin>("BackUpAndSpin");
00162 factory.registerNodeType<ComputePathToPose>("ComputePathToPose");
00163 factory.registerNodeType<FollowPath>("FollowPath");
00164
00165 auto tree = factory.createTreeFromText(xml_text);
00166
00167
00168
00169 IsStuck *first_stuck_node = nullptr;
00170 IsStuck *second_stuck_node = nullptr;
00171 BackUpAndSpin* back_spin_node = nullptr;
00172 ComputePathToPose* compute_node = nullptr;
00173 FollowPath* follow_node = nullptr;
00174
00175 for (auto& node: tree.nodes)
00176 {
00177 auto ptr = node.get();
00178
00179 if( !first_stuck_node )
00180 {
00181 TryDynamicCastPtr(ptr, first_stuck_node);
00182 }
00183 else{
00184 TryDynamicCastPtr(ptr, second_stuck_node);
00185 }
00186 TryDynamicCastPtr(ptr, back_spin_node);
00187 TryDynamicCastPtr(ptr, follow_node);
00188 TryDynamicCastPtr(ptr, compute_node);
00189 }
00190
00191 ASSERT_TRUE( first_stuck_node );
00192 ASSERT_TRUE( second_stuck_node );
00193 ASSERT_TRUE( back_spin_node );
00194 ASSERT_TRUE( compute_node );
00195 ASSERT_TRUE( follow_node );
00196
00197 std::cout << "-----------------------" << std::endl;
00198
00199
00200 NodeStatus status = NodeStatus::IDLE;
00201
00202 first_stuck_node->setExpectedResult(false);
00203
00204 while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
00205 {
00206 status = tree.root_node->executeTick();
00207 std::this_thread::sleep_for(Milliseconds(100));
00208 }
00209
00210
00211 ASSERT_EQ( status, NodeStatus::SUCCESS );
00212
00213 ASSERT_GE( first_stuck_node->tickCount(), 6);
00214
00215 ASSERT_EQ( second_stuck_node->tickCount(), 0 );
00216 ASSERT_EQ( back_spin_node->tickCount(), 0 );
00217
00218 ASSERT_EQ( compute_node->tickCount(), 1 );
00219 ASSERT_EQ( follow_node->tickCount(), 1 );
00220 ASSERT_FALSE( follow_node->wasHalted() );
00221
00222 std::cout << "-----------------------" << std::endl;
00223
00224
00225
00226
00227 first_stuck_node->resetTickCount();
00228 second_stuck_node->resetTickCount();
00229 compute_node->resetTickCount();
00230 follow_node->resetTickCount();
00231 back_spin_node->resetTickCount();
00232 status = NodeStatus::IDLE;
00233 int cycle = 0;
00234
00235 while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
00236 {
00237
00238 if( ++cycle == 2 )
00239 {
00240 first_stuck_node->setExpectedResult(true);
00241 second_stuck_node->setExpectedResult(true);
00242 }
00243 status = tree.root_node->executeTick();
00244 std::this_thread::sleep_for(Milliseconds(100));
00245 }
00246
00247
00248 ASSERT_EQ( status, NodeStatus::SUCCESS );
00249
00250
00251 ASSERT_GE( first_stuck_node->tickCount(), 2);
00252
00253 ASSERT_EQ( second_stuck_node->tickCount(), 1 );
00254 ASSERT_EQ( back_spin_node->tickCount(), 1 );
00255
00256
00257 ASSERT_EQ( compute_node->tickCount(), 1 );
00258
00259 ASSERT_EQ( follow_node->tickCount(), 0 );
00260 ASSERT_TRUE( follow_node->wasHalted() );
00261
00262 ASSERT_EQ( compute_node->status(), NodeStatus::IDLE );
00263 ASSERT_EQ( follow_node->status(), NodeStatus::IDLE );
00264 ASSERT_EQ( back_spin_node->status(), NodeStatus::IDLE );
00265
00266
00267 std::cout << "-----------------------" << std::endl;
00268
00269
00270
00271
00272 first_stuck_node->resetTickCount();
00273 second_stuck_node->resetTickCount();
00274 compute_node->resetTickCount();
00275 follow_node->resetTickCount();
00276 back_spin_node->resetTickCount();
00277 status = NodeStatus::IDLE;
00278 first_stuck_node->setExpectedResult(false);
00279 second_stuck_node->setExpectedResult(false);
00280
00281 while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
00282 {
00283 status = tree.root_node->executeTick();
00284 std::this_thread::sleep_for(Milliseconds(100));
00285 }
00286
00287
00288 ASSERT_EQ( status, NodeStatus::SUCCESS );
00289
00290 ASSERT_GE( first_stuck_node->tickCount(), 6);
00291 ASSERT_EQ( second_stuck_node->tickCount(), 0 );
00292 ASSERT_EQ( back_spin_node->tickCount(), 0 );
00293
00294 ASSERT_EQ( compute_node->status(), NodeStatus::IDLE );
00295 ASSERT_EQ( follow_node->status(), NodeStatus::IDLE );
00296 ASSERT_EQ( back_spin_node->status(), NodeStatus::IDLE );
00297 ASSERT_FALSE( follow_node->wasHalted() );
00298
00299 }
00300
00301