Function TEST

tests/navigation_test.cpp:181–326

/****************TESTS START HERE***************************/

TEST(Navigationtest, MoveBaseRecovery)
{
  BehaviorTreeFactory factory;

  factory.registerNodeType<IsStuck>("IsStuck");
  factory.registerNodeType<BackUpAndSpin>("BackUpAndSpin");
  factory.registerNodeType<ComputePathToPose>("ComputePathToPose");
  factory.registerNodeType<FollowPath>("FollowPath");

  auto tree = factory.createTreeFromText(xml_text);

  // Need to retrieve the node pointers with dynamic cast
  // In a normal application you would NEVER want to do such a thing.
  IsStuck* first_stuck_node = nullptr;
  IsStuck* second_stuck_node = nullptr;
  BackUpAndSpin* back_spin_node = nullptr;
  ComputePathToPose* compute_node = nullptr;
  FollowPath* follow_node = nullptr;

  for(auto& subtree : tree.subtrees)
  {
    for(auto& node : subtree->nodes)
    {
      auto ptr = node.get();

      if(first_stuck_node == nullptr)
      {
        TryDynamicCastPtr(ptr, first_stuck_node);
      }
      else
      {
        TryDynamicCastPtr(ptr, second_stuck_node);
      }
      TryDynamicCastPtr(ptr, back_spin_node);
      TryDynamicCastPtr(ptr, follow_node);
      TryDynamicCastPtr(ptr, compute_node);
    }
  }

  ASSERT_TRUE(first_stuck_node);
  ASSERT_TRUE(second_stuck_node);
  ASSERT_TRUE(back_spin_node);
  ASSERT_TRUE(compute_node);
  ASSERT_TRUE(follow_node);

  std::cout << "-----------------------" << std::endl;

  // First case: not stuck, everything fine.
  NodeStatus status = NodeStatus::IDLE;

  first_stuck_node->setExpectedResult(false);

  while(status == NodeStatus::IDLE || status == NodeStatus::RUNNING)
  {
    status = tree.tickWhileRunning();
    std::this_thread::sleep_for(Milliseconds(100));
  }