int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/GBS
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
argv[4]=NUMBER_OF_ROBOTS //this is only necessary to automatically define G2
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
//printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
/** Define G1 and G2 **/
double G1 = 0.1;
int NUMBER_OF_ROBOTS = atoi(argv[4]);
//default:
double G2 = 100.0;
double edge_min = 1.0;
if ( strcmp (graph_file,"maps/grid/grid.graph") == 0 ){
if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;}
if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;}
if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;}
if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;}
if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;}
if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;}
}else if (strcmp (graph_file,"maps/example/example.graph") == 0 ) {
if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;}
if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;}
if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;}
edge_min = 20.0;
}else if (strcmp (graph_file,"maps/cumberland/cumberland.graph") == 0) {
if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;}
if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;}
if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;}
edge_min = 50.0;
}
printf("G1 = %f, G2 = %f\n", G1, G2);
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_reactive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("initial position: x = %f, y = %f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("initial vertex = %d\n\n",current_vertex);
//INITIALIZE tab_intention:
int tab_intention[NUMBER_OF_ROBOTS];
for (i=0; i<NUMBER_OF_ROBOTS; i++){
tab_intention[i] = -1;
}
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados de "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
/* Define Goal */
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex
}
}
interference = false;
ResendGoal = false;
goal_complete = true;
double now;
while(1){
if(goal_complete){
if(next_vertex>-1){
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
} */
}
//devolver proximo vertex tendo em conta apenas as idlenesses;
next_vertex = (int) state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min);
//printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
/** SEND INTENTION **/
send_intention(next_vertex);
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat;
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.waitForResult();
goal_complete = false;
}else{
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if (arrived && NUMBER_OF_ROBOTS>1){ //a different robot arrived at a vertex: update idleness table and keep track of last vertices positions of other robots.
//printf("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == vertex_arrived){
//actualizar last_visit[dimension]
last_visit[vertex_arrived] = now;
}
//actualizar instantaneous_idleness[dimension]
instantaneous_idleness[i]= now - last_visit[i];
}
//Show Idleness Table:
// for (i=0; i<dimension; i++){
// printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
// }
arrived = false;
}
if (intention && NUMBER_OF_ROBOTS>1){
tab_intention[robot_intention] = vertex_intention;
//printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
intention = false;
}
if(end_simulation){
return 0;
}
}
}
return 0;
}
int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1);
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
uint j;
/* Output Graph Data */
for (i=0;i<dimension;i++){
printf ("ID= %u\n", vertex_web[i].id);
printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
for (j=0;j<vertex_web[i].num_neigh; j++){
printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
}
printf("\n");
}
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_cognitive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("x=%f, y=%f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("v=%d\n",current_vertex);
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados da propria "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
// Define Goal
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base"
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string do move base = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
//instantaneous idleness and last visit initialized with zeros:
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away
}
}
bool inpath = false;
uint path [dimension];
uint elem_s_path=0, i_path=0;
interference = false;
ResendGoal = false;
goal_complete = true;
// printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
while(1){
if (goal_complete){
if (i_path>0){ //nao faz update no inicio
//Update Idleness Table:
double now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
}
*/
}
if(inpath){
//The robot is on its way to a global objective -> get NEXT_VERTEX from its path:
i_path++; //desde que nao passe o tamanho do path
if (i_path<elem_s_path){
next_vertex=path[i_path];
}else{
inpath = false;
}
}
if (!inpath){
elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path);
/* printf("Path: ");
for (i=0;i<elem_s_path;i++){
if (i==elem_s_path-1){
printf("%u.\n",path[i]);
}else{
printf("%u, ",path[i]);
}
}
*/
//we have the path and the number of elements in the path
i_path=1;
next_vertex = path[i_path];
inpath = true;
// printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
}
if (inpath){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.sendGoal(goal);
//ac.waitForResult();
}
goal_complete = false; //garantir q n volta a entrar a seguir aqui
// printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}else{ //goal not complete (active)
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if(end_simulation){
return 0;
}
// printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
}
// return 0;
}
/**
* Returns the property associated with the graph
* @return the graph property
*/
inline
OpGraphInfoRef CGetOpGraphInfo()
{
return RefcountCast<OpGraphInfo>(GetGraphInfo());
}
