#include "movebase_node.h"

#include "behaviortree_cpp/bt_factory.h"

// This function must be implemented in the .cpp file to create

// a plugin that can be loaded at run-time

BT_REGISTER_NODES(factory)

{

factory.registerNodeType<MoveBaseAction>("MoveBase");

}

BT::NodeStatus MoveBaseAction::onStart()

{

if(!getInput<Pose2D>("goal", _goal))

{

throw BT::RuntimeError("missing required input [goal]");

}

printf("[ MoveBase: SEND REQUEST ]. goal: x=%.1f y=%.1f theta=%.1f\n", _goal.x, _goal.y,

_goal.theta);

// We use this counter to simulate an action that takes a certain

// amount of time to be completed (220 ms)

_completion_time = chr::system_clock::now() + chr::milliseconds(220);

return BT::NodeStatus::RUNNING;

}

BT::NodeStatus MoveBaseAction::onRunning()

{

// Pretend that we are checking if the reply has been received

// you don't want to block inside this function too much time.

std::this_thread::sleep_for(chr::milliseconds(10));

// Pretend that, after a certain amount of time,

// we have completed the operation

if(chr::system_clock::now() >= _completion_time)

{

std::cout << "[ MoveBase: FINISHED ]" << std::endl;

return BT::NodeStatus::SUCCESS;

}

return BT::NodeStatus::RUNNING;

}

void MoveBaseAction::onHalted()

{

printf("[ MoveBase: ABORTED ]");

}