|
14 | 14 | #include "action_test_node.h"
|
15 | 15 | #include "condition_test_node.h"
|
16 | 16 | #include "behaviortree_cpp_v3/behavior_tree.h"
|
| 17 | +#include "behaviortree_cpp_v3/bt_factory.h" |
17 | 18 |
|
18 | 19 | using BT::NodeStatus;
|
19 | 20 | using std::chrono::milliseconds;
|
@@ -310,3 +311,53 @@ TEST_F(ComplexFallbackWithMemoryTest, Action1Failed)
|
310 | 311 | ASSERT_EQ(NodeStatus::FAILURE, action_1.status());
|
311 | 312 | ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
|
312 | 313 | }
|
| 314 | + |
| 315 | + |
| 316 | +TEST(FallbackAndRetry, FallbackAndRetry) |
| 317 | +{ |
| 318 | + using namespace BT; |
| 319 | + static const char* xml_text = R"( |
| 320 | +
|
| 321 | + <root main_tree_to_execute="BehaviorTree"> |
| 322 | + <BehaviorTree ID="BehaviorTree"> |
| 323 | + <RetryUntilSuccessful num_attempts="2"> |
| 324 | + <Fallback> |
| 325 | + <AsyncActionTest name="first"/> |
| 326 | + <ForceFailure> |
| 327 | + <AsyncActionTest name="second"/> |
| 328 | + </ForceFailure> |
| 329 | + </Fallback> |
| 330 | + </RetryUntilSuccessful> |
| 331 | + </BehaviorTree> |
| 332 | + </root> |
| 333 | + )"; |
| 334 | + |
| 335 | + BehaviorTreeFactory factory; |
| 336 | + factory.registerNodeType<AsyncActionTest>("AsyncActionTest"); |
| 337 | + |
| 338 | + auto tree = factory.createTreeFromText(xml_text); |
| 339 | + |
| 340 | + std::vector<AsyncActionTest*> async; |
| 341 | + |
| 342 | + for(auto node: tree.nodes) { |
| 343 | + if(auto async_node = dynamic_cast<AsyncActionTest*>(node.get()) ) |
| 344 | + { |
| 345 | + async.push_back(async_node); |
| 346 | + } |
| 347 | + } |
| 348 | + |
| 349 | + ASSERT_EQ(async.size(), 2); |
| 350 | + async[0]->setExpectedResult(NodeStatus::FAILURE); |
| 351 | + async[1]->setExpectedResult(NodeStatus::SUCCESS); |
| 352 | + |
| 353 | + auto res = tree.tickRootWhileRunning(); |
| 354 | + |
| 355 | + ASSERT_EQ(async[0]->failureCount(), 2); |
| 356 | + ASSERT_EQ(async[0]->successCount(), 0); |
| 357 | + |
| 358 | + ASSERT_EQ(async[1]->failureCount(), 0); |
| 359 | + ASSERT_EQ(async[1]->successCount(), 2); |
| 360 | + |
| 361 | + ASSERT_EQ(NodeStatus::FAILURE, res); |
| 362 | +} |
| 363 | + |
0 commit comments