int main(int argc, char **argv)
{
Route path;
//path.brickDelivery();
path.brickOrder(CELL_1);
path.infoRoute();
ros::init(argc, argv, "mission_node");
ros::NodeHandle nodeHandler;
nodeHandler.param<int>("loopRate", loopRate, 10);
//init publishers
action_publisher = nodeHandler.advertise<msgs::IntStamped>("mission/next_mission",1);
ros::Rate rate(loopRate);
/*
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_navigation("fibonacci", true);
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_to_cell("fibonacci", true);
actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> action_from_cell("fibonacci", true);
action_navigation.waitForServer();
action_to_cell.waitForServer();
action_from_cell.waitForServer();
learning_actionlib::FibonacciGoal goal;
goal.order = 20;
ac.sendGoal(goal);
*/
while(ros::ok())
{
//check mes order
/*if(mes=getBricks)
{
path.brickOrder(CELL);
}
if(mes=Delivery)
{
path.brickDelivery();
}*/
if(path.empty() && path.getCurrentState() != CHARGE)
{
path.goCharge();
}
if(!path.empty() && path.next() != CTR_IDLE)
{
if(navigation_area)
{
if(path.next() == TRANSITION)
{
navigation_area = false;
}
//learning_actionlib::FibonacciGoal goal;
//goal.order = path.next();
//action_navigation.sendGoal(goal);
//action_navigation.waitForResult(); //default 0, which should mean blocking
path.pop();
}
else
{
if(path.next() == TRANSITION)
{
navigation_area = true;
//learning_actionlib::FibonacciGoal goal;
//action_from_cell.sendGoal();
//action_from_cell.waitForResult(); //default 0, which should mean blocking
path.pop();
}
else
{
//learning_actionlib::FibonacciGoal goal;
//goal.order = path.next();
//action_to_cell.sendGoal(goal);
//action_to_cell.waitForResult(); //default 0, which should mean blocking
path.pop();
}
}
}
msgs::IntStamped gui_message;
gui_message.header.stamp = ros::Time::now();
gui_message.data = path.next();
action_publisher.publish(gui_message);
ros::spinOnce();
rate.sleep();
}
return 0;
}
