STATE NodeROS::execute()
{
set_highlighted(true);
glut_process();
std::cout << "NodeROS::execute()" << std::endl;
if (overwritten_)
{
set_highlighted(false);
return node_status_ = FAILURE; //overwritten_result_;
}
else
{
bool finished;
// received_ = false;
{
boost::lock_guard<boost::mutex> lock(mutex_finished_);
finished = finished_;
}
ROS_INFO("RECEIVED: %d", received_);
if (!finished && !received_)
{
std::cout << "Sending Goal Client: "
<< ros_node_name_ << std::endl;
behavior_trees::ROSGoal goal;
goal.GOAL_ = 1; // possitive tick
{
boost::lock_guard<boost::mutex> lock(mutex_active_);
active_ = false;
}
ac_.sendGoal(goal,
boost::bind(&NodeROS::doneCb, this, _1, _2),
boost::bind(&NodeROS::activeCb, this),
boost::bind(&NodeROS::feedbackCb, this, _1));
std::cout << "Waiting for Feedback at Node: " << this << std::endl;
while (!received_ && !active_)
{
sleep(0.01);
// std::cout << "*";
}
std::cout << "Received Feedback at Node: " << this << std::endl;
}
{
boost::lock_guard<boost::mutex> lock(mutex_received_);
received_ = false;
}
{
boost::lock_guard<boost::mutex> lock(mutex_node_status_);
std::cout << "STATUS: " << node_status_ << std::endl;
set_highlighted(false);
return node_status_;
}
}
}
int main(int argc, char** argv)
{
std::cout << "Hello, world!" << std::endl;
// setup signal interrupt handler
signal(SIGINT, signal_callback_handler);
// specify which options are available as cmd line arguments
setupCmdLineReader();
// read agent id from command line parameters (--agent=mario)
agent = readAgentFromCmdLine(argc, argv);
// initialize the behavior tree client node
ros::init(argc, argv, std::string("behavior_tree") + "_" + agent);
// initialize OpenGL engine for visualization
glut_setup(argc, argv);
// point to the root of the behavior tree
node_cursor = node = &root;
node_cursor->set_highlighted(true);
// create the bt from the file bt.txt (put on the path)
std::cout << "----------------- PARSE FILE -----------------" << std::endl;
parse_file(std::string("bt") + "_" + agent + ".txt");
// print the data parsed from the specification file
std::cout << "----------------- PRINT TREE -----------------" << std::endl;
root.print_subtree();
// wait until user inputs Enter to start
std::cout << "Press Enter To Start" << std::endl;
std::cin.get();
// start ticking the root of the tree at frequency: TICK_FREQUENCY
while (ros::ok())
{
std::cout << "**** run" << run << std::endl;
std::cout << "-------------- EXECUTE TREE --------------" << std::endl;
root.execute_reset_status();
root.execute(); // sending tick
get_keypressed(); // processing keystrokes
process_keypressed();
glut_process(); // update visualization
glutPostRedisplay();
ros::Duration(1.0/TICK_FREQUENCY).sleep();
std::cout << "**** run" << run << std::endl;
}
// missing to clear the dynamically allocated tree
// delete_tree();
return 0;
}