Heuristic_Conscientious_Reactive.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 int next_vertex = -1;
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...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/Heuristic_Conscientious_Reactive
00339   argv[1]=__name:=XXXXXX
00340   argv[2]=maps/1r-5-map.graph
00341   argv[3]=ID_ROBOT
00342   */
00343   
00344   
00345   //More than One robot (ID between 0 and 99)
00346   if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
00347     ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots 
00348     return 0;
00349   }else{
00350     ID_ROBOT = atoi(argv[3]); 
00351     //printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
00352   }
00353 
00354   uint i = strlen( argv[2] );
00355   char graph_file[i];
00356   strcpy (graph_file,argv[2]);
00357   
00358   //Check Graph Dimension:
00359   uint dimension = GetGraphDimension(graph_file);
00360   
00361   //Create Structure to save the Graph Info;
00362   vertex vertex_web[dimension];
00363   
00364   //Get the Graph info from the Graph File
00365   GetGraphInfo(vertex_web, dimension, graph_file);
00366   
00367   uint j;
00368   
00369   
00370   
00371   /* Output Graph Data */
00372   for (i=0;i<dimension;i++){
00373     printf ("ID= %u\n", vertex_web[i].id);
00374     printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
00375     printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
00376         
00377     for (j=0;j<vertex_web[i].num_neigh; j++){
00378       printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
00379     }
00380     
00381     printf("\n");       
00382   }
00383   
00384   
00385   
00386   /* Define Starting Vertex/Position (Launch File Parameters) */
00387 
00388   ros::init(argc, argv, "hc_reactive");
00389   ros::NodeHandle nh;
00390   double initial_x, initial_y;
00391   
00392   XmlRpc::XmlRpcValue list;
00393   nh.getParam("initial_pos", list);
00394   ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00395   
00396   int value = ID_ROBOT;
00397   if (value == -1){value = 0;}
00398   
00399   ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00400   initial_x = static_cast<double>(list[2*value]);
00401   
00402   ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00403   initial_y = static_cast<double>(list[2*value+1]);
00404  
00405 //    printf("initial position: x = %f, y = %f\n", initial_x, initial_y);
00406    uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
00407 //    printf("initial vertex = %d\n\n",current_vertex);
00408   
00409    //Publicar dados de "odom" para nó de posições
00410   odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
00411         
00412   //Subscrever posições de outros robots
00413   odom_sub = nh.subscribe("positions", 10, positionsCB);  
00414   
00415   char string[20];
00416   char string2[20];  
00417   
00418   if(ID_ROBOT==-1){ 
00419     strcpy (string,"odom"); //string = "odom"
00420     strcpy (string2,"cmd_vel"); //string = "cmd_vel"
00421     TEAMSIZE = 1;
00422   }else{ 
00423     strcpy (string,"robot_"); 
00424     strcpy (string2,"robot_");     
00425     char id[3];
00426     itoa(ID_ROBOT, id, 10);  
00427     strcat(string,id);
00428     strcat(string2,id);
00429     strcat(string,"/odom"); //string = "robot_X/odom" 
00430     strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
00431     TEAMSIZE = ID_ROBOT + 1;
00432   }       
00433   
00434 //   printf("string de publicação da odometria: %s\n",string);
00435 
00436    //Cmd_vel to backup:
00437    cmd_vel_pub  = nh.advertise<geometry_msgs::Twist>(string2, 1);
00438   
00439   //Subscrever para obter dados de "odom" do robot corrente
00440   ros::Subscriber sub;
00441   sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
00442   ros::spinOnce();      
00443   
00444   
00445   /* Define Goal */  
00446 
00447   if(ID_ROBOT==-1){ 
00448     strcpy (string,"move_base"); //string = "move_base"  
00449   }else{ 
00450     strcpy (string,"robot_"); 
00451     char id[3];
00452     itoa(ID_ROBOT, id, 10);  
00453     strcat(string,id);
00454     strcat(string,"/move_base"); //string = "robot_X/move_base"  
00455   }
00456   
00457   //printf("string = %s\n",string);
00458   MoveBaseClient ac(string, true); 
00459   
00460   //wait for the action server to come up
00461   while(!ac.waitForServer(ros::Duration(5.0))){
00462      ROS_INFO("Waiting for the move_base action server to come up");
00463   }  
00464 
00465   //Define Goal:
00466   move_base_msgs::MoveBaseGoal goal;
00467     
00468   //Publicar dados para "results"
00469   results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
00470   results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
00471   
00472   initialize_node(); //dizer q está vivo
00473   ros::Rate loop_rate(1); //1 segundo
00474   
00475   /* Wait until all nodes are ready.. */
00476   while(initialize){
00477         ros::spinOnce();
00478         loop_rate.sleep();
00479   }  
00480   
00481   /* Run Algorithm */
00482   
00483   //instantaneous idleness and last visit initialized with zeros:
00484   double instantaneous_idleness [dimension];
00485   double last_visit [dimension];
00486   for(i=0;i<dimension;i++){ 
00487     instantaneous_idleness[i]= 0.0; 
00488     last_visit[i]= 0.0; 
00489     
00490     if(i==current_vertex){
00491       last_visit[i]= 0.1; //Avoids getting back at the initial vertex
00492     }
00493   } 
00494 
00495   interference = false;
00496   ResendGoal = false;
00497   goal_complete = true;
00498  
00499   
00500   while(1){
00501 
00502         if(goal_complete){
00503                   
00504                   if (next_vertex>-1){
00505                         //Update Idleness Table:
00506                         double now = ros::Time::now().toSec();
00507                         
00508                         for(i=0; i<dimension; i++){
00509                         if (i == next_vertex){
00510                                 last_visit[i] = now;    
00511                         }       
00512                         instantaneous_idleness[i]= now - last_visit[i];           
00513                         } 
00514                                 
00515                         current_vertex = next_vertex;
00516                         
00517                         //Show Idleness Table:
00518         /*              for (i=0; i<dimension; i++){
00519                         printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);      
00520                         }  
00521         */        }
00522                   
00523                 //devolver proximo vertex tendo em conta apenas as idlenesses individuais;
00524                 next_vertex = (int) heuristic_conscientious_reactive(current_vertex, vertex_web, instantaneous_idleness);
00525                 //printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00526                 
00527                 //Send the goal to the robot (Global Map)
00528                 geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
00529                 goal.target_pose.header.frame_id = "map"; 
00530                 goal.target_pose.header.stamp = ros::Time::now();    
00531                 goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
00532                 goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
00533                 goal.target_pose.pose.orientation = angle_quat;
00534                 ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00535                 ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
00536                 //ac.sendGoal(goal);
00537                 //ac.waitForResult();
00538 
00539                 /* if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
00540                 ROS_INFO("Hooray, the base moved to the correct point.");
00541                 }else{
00542                 ROS_INFO("The base failed to move for some reason.");    
00543                 return 0;
00544                 }*/
00545                 
00546                 goal_complete = false;
00547                 
00548          }else{
00549                 if (interference){
00550                                                         
00551                         // Stop the robot..                     
00552                         ROS_INFO("Interference detected!\n");   
00553                         send_interference();
00554 
00555                         //get own "odom" positions...
00556                         ros::spinOnce();                
00557                                                 
00558                         //Waiting until conflict is solved...
00559                         while(interference){
00560                                 interference = check_interference();
00561                                 if (goal_complete || ResendGoal){
00562                                         interference = false;
00563                                 }
00564                         }
00565                                 // se saiu é pq interference = false                   
00566                 }           
00567                 
00568                 if(ResendGoal){
00569                         //Send the goal to the robot (Global Map)
00570                         geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);         
00571                         goal.target_pose.header.frame_id = "map"; 
00572                         goal.target_pose.header.stamp = ros::Time::now();    
00573                         goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
00574                         goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
00575                         goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
00576                         ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00577         //              printf("ID_ROBOT = %d\n", ID_ROBOT);
00578                         ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
00579                         ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
00580                 }
00581                 
00582                 if(end_simulation){
00583                         return 0;
00584                 }               
00585                 
00586         }
00587   }
00588   return 0; 
00589 }


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