simple_coro_test.cpp

#include <behaviortree_ros_actions/do_ros_action.h>
#include <behaviortree_cpp/decorators/timeout_node.h>
#include <gtest/gtest.h>

using namespace std::chrono_literals;

class SimpleCoroAction : public BT::CoroActionNode
{
public:
  SimpleCoroAction(std::chrono::milliseconds timeout, bool will_fail,
                   const std::string &node_name, const BT::NodeConfiguration &config)
    : BT::CoroActionNode(node_name, config)
    , timeout_(timeout)
    , will_fail_(will_fail)
    , start_time_(std::chrono::time_point<std::chrono::steady_clock>::min())
  {
  }

  virtual void halt() override
  {
    std::cout << "Action was halted" << std::endl;
    BT::CoroActionNode::halt();
  }

protected:
  virtual BT::NodeStatus tick() override
  {
    std::cout << "Starting action " << std::endl;

    if (start_time_ == std::chrono::time_point<std::chrono::steady_clock>::min())
      start_time_ = std::chrono::steady_clock::now();

    while (std::chrono::steady_clock::now() < (start_time_ + timeout_))
    {
      setStatusRunningAndYield();
    }

    start_time_ = std::chrono::time_point<std::chrono::steady_clock>::min();
    return (will_fail_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS);
  }


public:
  std::chrono::milliseconds timeout_;
  bool will_fail_;

private:
  std::chrono::time_point<std::chrono::steady_clock> start_time_;
};


BT::NodeStatus executeCoRoutine(BT::TreeNode &node)
{
  auto status = node.executeTick();
  while (status == BT::NodeStatus::RUNNING)
  {
    status = node.executeTick();
  }
  return status;
}

TEST(SimpleCoroTest, do_action)
{
  BT::NodeConfiguration node_config_;
  node_config_.blackboard = BT::Blackboard::create();
  BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
  SimpleCoroAction node(1000ms, false, "Action", node_config_);

  EXPECT_EQ(BT::NodeStatus::SUCCESS, executeCoRoutine(node));
  EXPECT_EQ(BT::NodeStatus::SUCCESS, executeCoRoutine(node)) << "Second call to coro action";
  node.will_fail_ = true;
  EXPECT_EQ(BT::NodeStatus::FAILURE, executeCoRoutine(node))
      << "Should execute again and retun failure";

  node.setStatus(BT::NodeStatus::IDLE);  // We are forced to set this to ensure the action
                                         // is run again
  EXPECT_EQ(BT::NodeStatus::FAILURE, executeCoRoutine(node))
      << "Shoudln't fail because we set status to idle";
}


TEST(SimpleCoroTest, do_action_timeout)
{
  BT::NodeConfiguration node_config_;
  node_config_.blackboard = BT::Blackboard::create();
  BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
  SimpleCoroAction node(1000ms, false, "Action", node_config_);

  BT::TimeoutNode timeout("TimeoutAction", 500);
  timeout.setChild(&node);
  EXPECT_EQ(BT::NodeStatus::FAILURE, executeCoRoutine(timeout)) << "should timeout";

  node.timeout_ = 0ms;
  node.setStatus(BT::NodeStatus::IDLE);  // Not sure if this is needed due to a bug
  EXPECT_EQ(BT::NodeStatus::SUCCESS, executeCoRoutine(timeout));
}

int main(int argc, char **argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}