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_;
           }
   }
}
示例#2
0
文件: main.cpp 项目: miccol/Nao-Demo
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;
}