Conscientious_Cognitive.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2011, ISR University of Coimbra.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the ISR University of Coimbra nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: David Portugal, 2011
00036 *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 #include <move_base_msgs/MoveBaseAction.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 #include <tf/transform_broadcaster.h>
00042 #include <nav_msgs/Odometry.h>
00043 
00044 #include "getgraph.h"
00045 #include "algorithms.h"
00046 
00047 #define NUM_MAX_ROBOTS 32
00048 
00049 typedef unsigned int uint;
00050 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00051 
00052 bool ResendGoal; //Send the same goal again (if goal failed...)
00053 bool interference;
00054 bool goal_complete;
00055 bool initialize = true;
00056 bool end_simulation = false;
00057 uint next_vertex;
00058 uint backUpCounter;
00059 
00060 int TEAMSIZE;
00061 int ID_ROBOT;
00062 
00063 double xPos[NUM_MAX_ROBOTS]; //tabelas de posições (atençao ao index pro caso de 1 so robot)
00064 double yPos[NUM_MAX_ROBOTS]; //tabelas de posições (atençao ao index pro caso de 1 so robot)
00065 
00066 ros::Subscriber odom_sub;
00067 ros::Publisher odom_pub;
00068 ros::Subscriber results_sub;
00069 ros::Publisher results_pub;
00070 ros::Publisher cmd_vel_pub;
00071 
00072 void send_goal_result (uint vertex){
00073   /*
00074   string frame_id       //robot ID
00075     float64 x           //Goal (X=vertex,0,0)  
00076 */      
00077         printf("Send Vertex %u [Goal] to Results: Robot %d\n",vertex,ID_ROBOT); 
00078         
00079         geometry_msgs::PointStamped msg;        
00080         char id_robot_str[3];
00081         sprintf(id_robot_str, "%d", ID_ROBOT);  //integer to array
00082                 
00083         msg.header.frame_id = id_robot_str;
00084         msg.point.x = float (vertex);
00085         msg.point.y = 0.0; 
00086         msg.point.z = 0.0;
00087 
00088         results_pub.publish(msg);
00089         ros::spinOnce();
00090 }
00091 
00092 void backup(){
00093         
00094         while (backUpCounter<=100){
00095         
00096         if(backUpCounter==0){
00097                 ROS_INFO("The wall is too close! I need to do some backing up...");
00098                 // Move the robot back...
00099                 geometry_msgs::Twist cmd_vel;
00100                 cmd_vel.linear.x = -0.1;
00101                 cmd_vel.angular.z = 0.0;
00102                 cmd_vel_pub.publish(cmd_vel);
00103         }
00104                         
00105         if(backUpCounter==40){
00106                 // Turn the robot around...
00107                 geometry_msgs::Twist cmd_vel;
00108                 cmd_vel.linear.x = 0.0;
00109                 cmd_vel.angular.z = 0.5;
00110                 cmd_vel_pub.publish(cmd_vel);
00111         }
00112                         
00113         if(backUpCounter==100){
00114                 // Stop the robot...
00115                 geometry_msgs::Twist cmd_vel;
00116                 cmd_vel.linear.x = 0.0;
00117                 cmd_vel.angular.z = 0.0;
00118                 cmd_vel_pub.publish(cmd_vel);
00119                         
00120                 ROS_INFO("Done backing up, now on with my life!");              
00121         }
00122 
00123         backUpCounter++;
00124         }
00125         
00126 }
00127 
00128 void goalDoneCallback(const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result){ //goal terminado (completo ou cancelado)
00129 //      ROS_INFO("Goal is complete (suceeded, aborted or cancelled).");
00130         // If the goal succeeded send a new one!
00131         //if(state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED) sendNewGoal = true;
00132         // If it was aborted time to back up!
00133         //if(state.state_ == actionlib::SimpleClientGoalState::ABORTED) needToBackUp = true;    
00134         
00135         if(state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED){
00136                 ROS_INFO("SUCCESS");
00137                 goal_complete = true;
00138                 send_goal_result (next_vertex);
00139                 
00140         }else{
00141                 ROS_INFO("CANCELLED or ABORTED...BACKUP & Resend Goal!");       //tentar voltar a enviar goal...
00142                 backUpCounter = 0;
00143                 backup();
00144                 ResendGoal = true;
00145         }
00146 }
00147 
00148 void goalActiveCallback(){      //enquanto o robot esta a andar para o goal...
00149         goal_complete = false;
00150 //      ROS_INFO("Goal is active.");
00151 }
00152 
00153 void odomCB(const nav_msgs::Odometry::ConstPtr& msg) { //colocar propria posicao na tabela
00154         
00155 //      printf("Colocar Propria posição na tabela, ID_ROBOT = %d\n",ID_ROBOT);
00156         int idx = ID_ROBOT;
00157         
00158         if (ID_ROBOT<=-1){
00159                 idx = 0;
00160         }
00161         
00162         xPos[idx]=msg->pose.pose.position.x;
00163         yPos[idx]=msg->pose.pose.position.y;
00164         
00165 //      printf("Posicao colocada em Pos[%d]\n",idx);
00166 }
00167 
00168 void positionsCB(const nav_msgs::Odometry::ConstPtr& msg) {     //construir tabelas de posições
00169         
00170 //     printf("Construir tabela de posicoes (receber posicoes), ID_ROBOT = %d\n",ID_ROBOT);    
00171         
00172     char id[20]; //identificador do robot q enviou a msg d posição...
00173     strcpy( id, msg->header.frame_id.c_str() );
00174     //int stamp = msg->header.seq;
00175 //     printf("robot q mandou msg = %s\n", id);
00176     
00177     // Build Positions Table
00178     
00179     if (ID_ROBOT>-1){
00180         //verify id "XX" of robot: (string: "robot_XX/odom")
00181         
00182         char str_idx[4];
00183         uint i;
00184         
00185         for (i=6; i<10; i++){
00186                 if (id[i]=='/'){
00187                         str_idx[i-6] = '\0';
00188                         break;
00189                 }else{
00190                         str_idx[i-6] = id[i];
00191                 }
00192         }
00193         
00194         int idx = atoi (str_idx);
00195 //      printf("id robot q mandou msg = %d\n",idx);
00196         
00197         if (idx >= TEAMSIZE && TEAMSIZE <= NUM_MAX_ROBOTS){
00198                 //update teamsize:
00199                 TEAMSIZE = idx+1;
00200         }
00201         
00202         if (ID_ROBOT != idx){  //Ignore own positions   
00203                 xPos[idx]=msg->pose.pose.position.x;
00204                 yPos[idx]=msg->pose.pose.position.y;            
00205         }       
00206 //      printf ("Position Table:\n frame.id = %s\n id_robot = %d\n xPos[%d] = %f\n yPos[%d] = %f\n\n", id, idx, idx, xPos[idx], idx, yPos[idx] );           
00207     }
00208 }
00209 
00210 void resultsCB(const geometry_msgs::PointStamped::ConstPtr& msg) { //
00211         
00212         if(initialize){ //em espera que o monitor a desbloquei e passe initialize para false
00213                 
00214 //              printf("Is it the monitor? frame_id = %s (%f,%f,%f)\n",msg->header.frame_id.c_str(),msg->point.x,msg->point.y,msg->point.z);
00215                 
00216                 if (msg->header.frame_id == "monitor"){
00217                         printf("Let's Patrol!\n");
00218                         initialize = false;
00219                 }
00220         }else {
00221                 if (msg->header.frame_id == "monitor" && msg->point.x == 1.0 && msg->point.y == 1.0 && msg->point.z == 1.0){
00222                         printf("The simulation is over. Let's leave\n");
00223                         end_simulation = true;
00224                 }               
00225         }
00226 }
00227 
00228 void initialize_node (){
00229   /*
00230   string frame_id       //robot ID
00231     float64 z           //initialization (0,0,Z=ID)  
00232 */      
00233         printf("Initialize Node: Robot %d\n",ID_ROBOT); 
00234         ros::Rate loop_rate(1); //1 segundo
00235         
00236         geometry_msgs::PointStamped msg;        
00237         
00238         char id_robot_str[3];
00239         sprintf(id_robot_str, "%d", ID_ROBOT);  //integer to array
00240                 
00241         msg.header.frame_id = id_robot_str;
00242         msg.point.x = 0.0;
00243         msg.point.y = 0.0; 
00244         msg.point.z = (float) ID_ROBOT; //Z = ID
00245         
00246         int count = 0;
00247         while (count<2){
00248                 results_pub.publish(msg);
00249                 ros::spinOnce();
00250                 //printf("publiquei msg.\n");
00251                 loop_rate.sleep();
00252                 count++;
00253         }       
00254 }
00255 
00256 void send_interference(){
00257   /*
00258   string frame_id       //robot ID
00259     float64 y           //Interference (0,Y=1,0)  
00260 */      
00261         printf("Send Interference: Robot %d\n",ID_ROBOT);       
00262         //ros::Rate loop_rate(1); //1 segundo
00263         
00264         geometry_msgs::PointStamped msg;        
00265         char id_robot_str[3];
00266         sprintf(id_robot_str, "%d", ID_ROBOT);  //integer to array
00267                 
00268         msg.header.frame_id = id_robot_str;
00269         msg.point.x = 0.0;
00270         msg.point.y = 1.0; 
00271         msg.point.z = 0.0;
00272 
00273         results_pub.publish(msg);
00274         ros::spinOnce();
00275 }
00276 
00277 bool check_interference (void){ //verificar se os robots estao proximos
00278         
00279         int i;
00280         double dist_quad;
00281         
00282         /* Poderei usar TEAMSIZE para afinar */
00283         for (i=0; i<ID_ROBOT; i++){     //percorrer vizinhos (assim asseguro q cada interferencia é so encontrada 1 vez)
00284                 
00285                 dist_quad = (xPos[i] - xPos[ID_ROBOT])*(xPos[i] - xPos[ID_ROBOT]) + (yPos[i] - yPos[ID_ROBOT])*(yPos[i] - yPos[ID_ROBOT]);
00286                 
00287                 if (dist_quad <= /*sqrt*/4){    //robots are 2 meter or less apart
00288 //                      ROS_INFO("Feedback: Robots are very close. INTERFERENCE! Dist_Quad = %f", dist_quad);
00289                         interference = true;
00290                         return interference;
00291                 }               
00292         }
00293         
00294         interference = false;
00295         return interference;
00296         
00297 }
00298 
00299 void goalFeedbackCallback(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback){    //publicar posições
00300 
00301 //      printf("ID_ROBOT = %d [feedback callback]\n", ID_ROBOT);
00302         
00303         //Publish Position to common node:
00304         nav_msgs::Odometry msg; 
00305         
00306         int idx = ID_ROBOT;
00307                 
00308         if (ID_ROBOT <= -1){
00309                 msg.header.frame_id = "odom";    //identificador do robot q publicou
00310                 idx = 0;
00311         }else{
00312                 char string[20];
00313                 strcpy (string,"robot_"); 
00314                 char id[3];
00315                 itoa(ID_ROBOT, id, 10);  
00316                 strcat(string,id);
00317                 strcat(string,"/odom"); //string = "robot_X/odom"       
00318                 //strcpy( msg.header.frame_id, string );
00319                 msg.header.frame_id = string;
00320         }
00321         
00322         msg.pose.pose.position.x = xPos[idx]; //send odometry.x
00323         msg.pose.pose.position.y = yPos[idx]; //send odometry.y
00324         
00325         odom_pub.publish(msg);
00326         ros::spinOnce();
00327         
00328         //ROS_INFO("[POSITION PUBLISHED]: ID = %s, X = %f, Y = %f", msg.header.frame_id.c_str(), xPos[idx], yPos[idx]);
00329 //      printf("ID_ROBOT = %d\n", ID_ROBOT);
00330 //      printf("TEAMSIZE = %d\n", TEAMSIZE);    
00331         check_interference();   
00332 }
00333 
00334 
00335 int main(int argc, char** argv){        //pass the .graph file to open
00336   /*
00337   argc=3
00338   argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive
00339   argv[1]=__name:=XXXXXX
00340   argv[2]=maps/1r-5-map.graph
00341   argv[3]=ID_ROBOT
00342   */
00343   
00344   //More than One robot (ID between 0 and 99)
00345   if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
00346     ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1);
00347      return 0;
00348   }else{
00349     ID_ROBOT = atoi(argv[3]); 
00350   }
00351   
00352 
00353   uint i = strlen( argv[2] );
00354   char graph_file[i];
00355   strcpy (graph_file,argv[2]);
00356   
00357   //Check Graph Dimension:
00358   uint dimension = GetGraphDimension(graph_file);
00359   
00360   //Create Structure to save the Graph Info;
00361   vertex vertex_web[dimension];
00362   
00363   //Get the Graph info from the Graph File
00364   GetGraphInfo(vertex_web, dimension, graph_file);
00365   
00366   uint j;
00367   
00368   
00369   
00370   /* Output Graph Data */
00371   for (i=0;i<dimension;i++){
00372     printf ("ID= %u\n", vertex_web[i].id);
00373     printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
00374     printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
00375         
00376     for (j=0;j<vertex_web[i].num_neigh; j++){
00377       printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
00378     }
00379     
00380     printf("\n");       
00381   }
00382   
00383   
00384   
00385   /* Define Starting Vertex/Position (Launch File Parameters) */
00386 
00387   ros::init(argc, argv, "c_cognitive");
00388   ros::NodeHandle nh;   
00389   double initial_x, initial_y;
00390   
00391   XmlRpc::XmlRpcValue list;
00392   nh.getParam("initial_pos", list);
00393   ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00394   
00395   int value = ID_ROBOT;
00396   if (value == -1){value = 0;}
00397   
00398   ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00399   initial_x = static_cast<double>(list[2*value]);
00400   
00401   ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00402   initial_y = static_cast<double>(list[2*value+1]);
00403  
00404 //   printf("x=%f, y=%f\n", initial_x, initial_y);
00405   uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
00406 //   printf("v=%d\n",current_vertex);
00407     
00408   //Publicar dados de "odom" para nó de posições
00409   odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
00410         
00411   //Subscrever posições de outros robots
00412   odom_sub = nh.subscribe("positions", 10, positionsCB); 
00413   
00414   char string[20];
00415   char string2[20];
00416         
00417   if(ID_ROBOT==-1){ 
00418     strcpy (string,"odom"); //string = "odom"
00419     strcpy (string2,"cmd_vel"); //string = "cmd_vel"
00420     TEAMSIZE = 1;
00421   }else{ 
00422     strcpy (string,"robot_"); 
00423     strcpy (string2,"robot_"); 
00424     char id[3];
00425     itoa(ID_ROBOT, id, 10);  
00426     strcat(string,id);
00427     strcat(string2,id);
00428     strcat(string,"/odom"); //string = "robot_X/odom"  
00429     strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
00430     TEAMSIZE = ID_ROBOT + 1;
00431   }       
00432   
00433 //   printf("string de publicação da odometria: %s\n",string);
00434 
00435    //Cmd_vel to backup:
00436    cmd_vel_pub  = nh.advertise<geometry_msgs::Twist>(string2, 1);
00437   
00438   //Subscrever para obter dados da propria "odom" do robot corrente
00439   ros::Subscriber sub;
00440   sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
00441   ros::spinOnce(); 
00442   
00443   
00444   // Define Goal 
00445 
00446   if(ID_ROBOT==-1){ 
00447     strcpy (string,"move_base"); //string = "move_base"  
00448   }else{ 
00449     strcpy (string,"robot_"); 
00450     char id[3];
00451     itoa(ID_ROBOT, id, 10);  
00452     strcat(string,id);
00453     strcat(string,"/move_base"); //string = "robot_X/move_base"  
00454   }
00455   
00456   //printf("string do move base = %s\n",string);
00457   MoveBaseClient ac(string, true); 
00458   
00459   //wait for the action server to come up
00460   while(!ac.waitForServer(ros::Duration(5.0))){
00461      ROS_INFO("Waiting for the move_base action server to come up");
00462   }  
00463 
00464   //Define Goal:
00465   move_base_msgs::MoveBaseGoal goal;                                                    
00466   
00467   //Publicar dados para "results"
00468   results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
00469   results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
00470   
00471   initialize_node(); //dizer q está vivo
00472   ros::Rate loop_rate(1); //1 segundo
00473   
00474   /* Wait until all nodes are ready.. */
00475   while(initialize){
00476         ros::spinOnce();
00477         loop_rate.sleep();
00478   }
00479   
00480   /* Run Algorithm */
00481   
00482   //instantaneous idleness and last visit initialized with zeros:
00483   double instantaneous_idleness [dimension];
00484   double last_visit [dimension];
00485   for(i=0;i<dimension;i++){ 
00486     instantaneous_idleness[i]= 0.0; 
00487     last_visit[i]= 0.0; 
00488     
00489     if(i==current_vertex){
00490       last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away
00491     }
00492   } 
00493 
00494 
00495   bool inpath = false;
00496   uint path [dimension];
00497   uint elem_s_path=0, i_path=0;
00498   
00499   interference = false;
00500   ResendGoal = false;
00501   goal_complete = true;
00502 
00503 //   printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
00504   
00505   while(1){
00506           
00507     if (goal_complete){
00508             
00509     if (i_path>0){      //nao faz update no inicio
00510       //Update Idleness Table:
00511       double now = ros::Time::now().toSec();
00512       
00513       for(i=0; i<dimension; i++){
00514         if (i == next_vertex){
00515           last_visit[i] = now;  
00516         }       
00517         instantaneous_idleness[i]= now - last_visit[i];           
00518       } 
00519           
00520       current_vertex = next_vertex;
00521       
00522       //Show Idleness Table:
00523 /*      for (i=0; i<dimension; i++){
00524         printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);      
00525        }
00526 */      
00527 
00528     }       
00529     
00530     if(inpath){
00531         //The robot is on its way to a global objective -> get NEXT_VERTEX from its path:
00532         i_path++; //desde que nao passe o tamanho do path
00533         
00534         if (i_path<elem_s_path){
00535           next_vertex=path[i_path];       
00536         }else{    
00537          inpath = false;        
00538         }
00539     }
00540     
00541     if (!inpath){
00542         elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path);
00543       
00544 /*      printf("Path: ");
00545       for (i=0;i<elem_s_path;i++){
00546         if (i==elem_s_path-1){
00547           printf("%u.\n",path[i]);
00548         }else{
00549           printf("%u, ",path[i]);
00550         }
00551       }
00552 */     
00553       //we have the path and the number of elements in the path
00554       i_path=1;
00555       next_vertex = path[i_path];
00556       inpath = true;
00557 //       printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00558     }
00559     
00560     if (inpath){
00561             
00562         geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);         
00563     
00564       //Send the goal to the robot (Global Map)
00565       goal.target_pose.header.frame_id = "map"; 
00566       goal.target_pose.header.stamp = ros::Time::now();    
00567       goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
00568       goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
00569       goal.target_pose.pose.orientation = angle_quat;   //alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
00570       ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00571       ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
00572       //ac.sendGoal(goal);
00573       //ac.waitForResult();
00574     }    
00575         
00576     goal_complete = false; //garantir q n volta a entrar a seguir aqui
00577 //     printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
00578     
00579     }else{ //goal not complete (active)
00580             
00581                 if (interference){
00582                                                 
00583                         // Stop the robot..                     
00584                         ROS_INFO("Interference detected!\n");   
00585                         send_interference();
00586 
00587                         //get own "odom" positions...
00588                         ros::spinOnce();                
00589                                         
00590                         //Waiting until conflict is solved...
00591                         while(interference){
00592                                 interference = check_interference();
00593                                 if (goal_complete || ResendGoal){
00594                                                 interference = false;
00595                                 }
00596                         }
00597                         // se saiu é pq interference = false                   
00598                 }           
00599             
00600             if(ResendGoal){
00601                 geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);         
00602     
00603                 //Send the goal to the robot (Global Map)
00604                 goal.target_pose.header.frame_id = "map"; 
00605                 goal.target_pose.header.stamp = ros::Time::now();    
00606                 goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
00607                 goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
00608                 goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
00609                 ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00610 //              printf("ID_ROBOT = %d\n", ID_ROBOT);
00611                 ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
00612                 ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
00613             }
00614             
00615             if(end_simulation){
00616                     return 0;
00617             }
00618             
00619 //          printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
00620             
00621     }
00622     
00623   }
00624   
00625 //   return 0; 
00626 }


patrolling_sim
Author(s): David Portugal
autogenerated on Mon Jan 6 2014 11:26:29