SSIPatrolAgent.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2014, 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: Luca Iocchi (2014-2016)
00036 *********************************************************************/
00037 
00038 #include "SSIPatrolAgent.h"
00039 
00040 using namespace std;
00041 
00042 SSIPatrolAgent::SSIPatrolAgent() : cf(CONFIG_FILENAME)
00043 {
00044     pthread_mutex_init(&lock, NULL);
00045 }
00046     
00047 
00048 void SSIPatrolAgent::onGoalComplete()
00049 {
00050     printf("DTAP onGoalComplete!!!\n");
00051     if (first_vertex){
00052         //printf("computing next vertex FOR THE FIRST TIME:\n current_vertex = %d, next_vertex=%d, next_next_vertex=%d",current_vertex, next_vertex,next_next_vertex);
00053         next_vertex = compute_next_vertex(current_vertex);
00054         //printf("DONE: current_vertex = %d, next_vertex=%d, next_next_vertex=%d\n",current_vertex, next_vertex,next_next_vertex);              
00055         first_vertex = false;
00056     } else {
00057         //printf("updating next vertex :\n current_vertex = %d, next_vertex=%d, next_next_vertex=%d\n",current_vertex, next_vertex,next_next_vertex);
00058         
00059         //Update Idleness Table:
00060         update_global_idleness();
00061         //update current vertex
00062         current_vertex = next_vertex;
00063         //update next vertex based on previous decision
00064         next_vertex = next_next_vertex;
00065         //update global idleness of next vertex to avoid conflicts
00066         
00067         if (next_vertex>=0 && next_vertex< dimension){
00068             pthread_mutex_lock(&lock);
00069             global_instantaneous_idleness[next_vertex] = 0.0;   
00070             pthread_mutex_unlock(&lock);
00071         }
00072         //printf("DONE: current_vertex = %d, next_vertex=%d, next_next_vertex=%d\n",current_vertex, next_vertex,next_next_vertex);              
00073    }
00074 
00076     send_goal_reached(); // Send TARGET to monitor
00077     send_results();  // Algorithm specific function
00078     
00079     //Send the goal to the robot (Global Map)
00080     ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00081     //sendGoal(vertex_web[next_vertex].x, vertex_web[next_vertex].y);  
00082     sendGoal(next_vertex);  // send to move_base
00083 
00084     goal_complete = false; 
00085 
00086     //compute next next vertex
00087     //printf("computing next_next_vertex :\n current_vertex = %d, next_vertex=%d, next_next_vertex=%d\n",current_vertex, next_vertex,next_next_vertex);
00088     
00089     next_next_vertex = compute_next_vertex(next_vertex); 
00090            
00091     printf("<<< DONE Computed next vertices: current_vertex = %d, next_vertex=%d, next_next_vertex=%d >>>\n",current_vertex, next_vertex,next_next_vertex);             
00092 
00093 }
00094 
00095 
00096 bool* SSIPatrolAgent::create_selected_vertices(){
00097         bool* sv = new bool[dimension];
00098         for(size_t i=0; i<dimension; i++) {
00099                 selected_vertices[i] = false;
00100         }       
00101         return sv;
00102 }
00103 
00104 void SSIPatrolAgent::reset_selected_vertices(bool* sv){
00105         for(size_t i=0; i<dimension; i++) {
00106                 sv[i] = false;
00107     }
00108     if (current_vertex >= 0 && current_vertex < dimension){
00109                 selected_vertices[current_vertex] = true; //do not consider next vertex as possible goal 
00110     }   
00111 }
00112 
00113 void SSIPatrolAgent::select_faraway_vertices(bool* sv, int cv){
00114         for(size_t i=0; i<dimension; i++) {
00115                 sv[i] = true;
00116     }
00117         uint num_neighs = vertex_web[cv].num_neigh;
00118     for (size_t i=0; i<num_neighs; i++){
00119       size_t neighbor = vertex_web[cv].id_neigh[i];
00120           sv[neighbor] = false;         
00121         }
00122     if (current_vertex >= 0 && current_vertex < dimension){
00123                 selected_vertices[current_vertex] = true; //do not consider next vertex as possible goal 
00124     }   
00125         /* print farway vertices
00126         printf("FARAWAY \n [ ");
00127         for(size_t i=0; i<dimension; i++) {
00128                 printf("%d ",sv[i]);
00129     }
00130         printf("]\n"); */
00131 }
00132 
00133 bool SSIPatrolAgent::all_selected(bool* sv){
00134         for(size_t i=0; i<dimension; i++) {
00135             if (!sv[i]){ 
00136                         return false;           
00137                 }       
00138     }
00139         return true;
00140 }
00141 
00142 
00143 void SSIPatrolAgent::init(int argc, char** argv) {
00144         
00145     PatrolAgent::init(argc,argv);
00146 
00147     //initialize structures
00148     next_vertex = -1; 
00149     next_next_vertex = -1;
00150 
00151     taskRequests = new int[dimension];  
00152     tasks = new bool[dimension];    
00153     bids = new bid_tuple[dimension]; 
00154     global_instantaneous_idleness = new double[dimension];
00155     selected_vertices = new bool[dimension];
00156     bid_tuple noBid = {BIG_NUMBER,-1}; 
00157     for(size_t i=0; i<dimension; i++) {
00158         taskRequests[i] = 0;
00159         tasks[i] = false;
00160         selected_vertices[i] = false;
00161         bids[i] = noBid;
00162         global_instantaneous_idleness[i]=dimension*2;  // start with a high value (not too high) 
00163     }
00164     nactivetasks=0;
00165 
00166     last_update_idl = ros::Time::now().toSec();
00167 
00168     first_vertex = true;        
00169 
00170     //initialize parameters
00171     timeout = cf.getDParam("timeout");
00172     theta_idl = cf.getDParam("theta_idleness");
00173     theta_cost = cf.getDParam("theta_navigation");
00174     theta_hop = cf.getDParam("theta_hop");      
00175     threshold = cf.getDParam("threshold");                      
00176     hist = cf.getDParam("hist");
00177 
00178     std::stringstream paramss;
00179     paramss << timeout << "," << theta_idl << "," << theta_cost << "," << theta_hop << "," << threshold << "," << hist;
00180 
00181     ros::param::set("/algorithm_params",paramss.str());
00182 
00183 }
00184 
00185 double SSIPatrolAgent::compute_cost(int vertex)
00186 {
00187     uint elem_s_path;
00188     int *shortest_path = new int[dimension]; 
00189     int id_neigh;
00190     
00191     dijkstra( current_vertex, vertex, shortest_path, elem_s_path, vertex_web, dimension); //structure with normal costs
00192     double distance = 0;
00193     
00194     for(uint j=0; j<elem_s_path; j++){
00195 //        printf("path[%u] = %d\n",j,shortest_path[j]);
00196         
00197         if (j<elem_s_path-1){
00198             id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00199             distance += vertex_web[shortest_path[j]].cost[id_neigh];
00200         }       
00201     }
00202     
00203     return distance;
00204 } 
00205         
00206 
00207 //TODO this should give the geometric distance from robot position to next vertex, and not path cost from current_vertex to next_vertex
00208 /*double SSIPatrolAgent::compute_distance(int vertex)
00209 {
00210 
00211     uint elem_s_path;
00212     int *shortest_path = new int[dimension]; 
00213     int id_neigh;
00214     
00215     dijkstra( current_vertex, vertex, shortest_path, elem_s_path, vertex_web, dimension); //structure with normal costs
00216     double distance = 0;
00217     
00218     for(uint j=0; j<elem_s_path; j++){
00219 //        printf("path[%u] = %d\n",j,shortest_path[j]);
00220         
00221         if (j<elem_s_path-1){
00222             id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00223             distance += vertex_web[shortest_path[j]].cost[id_neigh];
00224         }       
00225     }
00226     
00227     return distance;
00228 }        
00229 */
00230 
00231 double SSIPatrolAgent::compute_cost(int cv, int nv)
00232 {
00233     uint elem_s_path;
00234     int *shortest_path = new int[dimension]; 
00235     int id_neigh;
00236     
00237     dijkstra( cv, nv, shortest_path, elem_s_path, vertex_web, dimension); //structure with normal costs
00238     double distance = 0;
00239     
00240     for(uint j=0; j<elem_s_path; j++){
00241 //        printf("path[%u] = %d\n",j,shortest_path[j]);
00242         
00243         if (j<elem_s_path-1){
00244             id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00245             distance += vertex_web[shortest_path[j]].cost[id_neigh];
00246         }       
00247     }
00248     
00249     return distance;
00250 }        
00251 
00252 
00253 size_t SSIPatrolAgent::compute_hops(int cv, int nv)
00254 {
00255     uint elem_s_path;
00256     int *shortest_path = new int[dimension]; 
00257     int id_neigh;
00258     
00259     dijkstra( cv, nv, shortest_path, elem_s_path, vertex_web, dimension); //structure with normal costs
00260     
00261 #if 1
00262     return elem_s_path-1;
00263 #else    
00264     size_t hops = 0;
00265     
00266     for(uint j=0; j<elem_s_path; j++){
00267 //        printf("path[%u] = %d\n",j,shortest_path[j]);
00268         
00269         if (j<elem_s_path-1){
00270             id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00271                         hops++;
00272         }       
00273     }
00274     return hops;
00275 #endif
00276 }        
00277 
00278 
00279 double SSIPatrolAgent::utility(int cv,int nv) {
00280     double idl = global_instantaneous_idleness[nv];
00281     
00282     size_t hops = compute_hops(cv,nv);
00283     double U = theta_idl * idl + theta_hop * hops * dimension;
00284 
00285     // double cost = compute_cost(cv,nv); ????  1 hop = 5 m
00286     // double U = theta_idl * idl + theta_navigation * cost;
00287 #if DEBUG_PRINT
00288     if (U>-1000)
00289         printf("  HOPSUtil:: node: %d --> U[%d] ( %.1f, %zu ) = %.1f\n",cv,nv,idl,hops,U);
00290 #endif
00291     return U;
00292 }
00293 
00294 
00295 void SSIPatrolAgent::update_global_idleness() 
00296 {   
00297     double now = ros::Time::now().toSec();
00298     
00299     pthread_mutex_lock(&lock);
00300     for(size_t i=0; i<dimension; i++) {
00301         global_instantaneous_idleness[i] += (now-last_update_idl);  // update value    
00302     }
00303     
00304     if (current_vertex>=0 && current_vertex<dimension){
00305         global_instantaneous_idleness[current_vertex] = 0.0;
00306     }
00307     pthread_mutex_unlock(&lock);
00308 
00309     last_update_idl = now;
00310 }
00311 
00312 int SSIPatrolAgent::return_next_vertex(int cv,bool* sv){
00313     double maxUtility = -1e9;
00314     int i_maxUtility = 0;
00315 
00316     for(size_t i=0; i<dimension; i++){
00317         
00318         double U = utility(cv,i);
00319         if (U > maxUtility && !sv[i]){
00320             maxUtility = U;
00321             i_maxUtility = i;
00322         }
00323         
00324     }
00325     
00326     int nv = i_maxUtility; // vertex_web[current_vertex].id_neigh[i_maxUtility];
00327     sv[nv] = true; //this vertex was considered as a next vertex
00328     printf("DTASSI: returned vertex = %d (U = %.2f)\n",nv,maxUtility);
00329     return nv;
00330 }
00331 
00332 int SSIPatrolAgent::select_next_vertex(int cv,bool* sv){
00333     
00334     double maxUtility = -1e9;
00335     int i_maxUtility = 0;
00336 
00337 
00338     //check if there are vertices not selected, if not reset all to false       
00339     if (all_selected(selected_vertices)) {
00340                 reset_selected_vertices(sv);
00341     }
00342     for(size_t i=0; i<dimension; i++){
00343         
00344         double U = utility(cv,i);
00345 //              printf("vertex %d, marked %d",i,sv[i]);
00346         if (U > maxUtility && !sv[i]){
00347             maxUtility = U;
00348             i_maxUtility = i;
00349         }
00350     }
00351     
00352     int nv = i_maxUtility; // vertex_web[current_vertex].id_neigh[i_maxUtility];
00353     sv[nv] = true; //this vertex was considered as a next vertex
00354     // printf("DTASSI: selected possible vertex = %d (U = %.2f)\n",nv,maxUtility);
00355 
00356     return nv;
00357 }
00358 
00359 double SSIPatrolAgent::compute_bid(int nv){
00360 
00361         printf("## computing bid for vertex %d \n",nv);
00362 /*      printf("current tasks = ");
00363         for (size_t i = 0; i<dimension;i++){
00364                 printf(" %d, ",tasks[i]);       
00365     }
00366     printf("] \n"); */
00367 
00368         if (nv==next_vertex || nv==next_next_vertex){
00369 //              printf("already going to %d sending 0 (current target: %d, current next target: %d)",nv,next_vertex,next_next_vertex);
00370                 return 0.;
00371         }
00372 
00373         //local copy of task list for computing the bid, put as selected the ones I do not want to consider 
00374         bool* my_tasks = new bool[dimension];
00375         //printf("current tasks [");
00376         for (size_t i = 0; i<dimension; i++){
00377         //      printf(" %d, ",tasks[i]);
00378                 my_tasks[i] = !tasks[i];
00379         } 
00380         //printf(" ] \n");*/
00381 
00382         //add nv to my_tasks (regardless of whether this was my responsibility already)
00383         my_tasks[nv] = false;
00384 
00385 /*      printf("my tasks [");
00386         for (size_t i = 0; i<dimension; i++){
00387                 printf(" %d, ",my_tasks[i]);
00388         } 
00389         printf(" ] \n");
00390 */
00391         //accumulator for total path cost
00392         double path_cost = 0.;
00393 
00394         //put as first location the current target if any
00395         int ci = -1;
00396         if (next_vertex != -1){
00397                 ci = next_vertex;
00398                 path_cost = compute_cost(ci);//this should give the geometric distance from robot position to destination 
00399                 my_tasks[ci] = true; //remove this task from the list
00400                 //printf("[Target Set] Pathcost from %d to %d : %.2f \n",current_vertex,ci,path_cost);
00401         } else { 
00402                 ci = return_next_vertex(current_vertex,my_tasks);
00403                 path_cost = compute_cost(ci);
00404                 //printf("[Target NOT Set] Pathcost from %d to %d : %.2f \n",current_vertex,ci,path_cost);
00405         }
00406         //handle remaining locations in reverse utility order
00407         while (!all_selected(my_tasks)){
00408                 int ni = return_next_vertex(ci,my_tasks);
00409                 path_cost += compute_cost(ci,ni);
00410                 //printf("[while loop] pathcost from %d to %d : %.2f \n",ci,ni,path_cost);
00411                 ci=ni;
00412         }
00413         if (ci >= 0 && ci < dimension){
00414                 printf("returning back from (last task) %d to (current vertex) %d (cost = %.2f) \n",ci,current_vertex,path_cost);
00415                 path_cost += compute_cost(ci,current_vertex);
00416         }
00417         printf("## total cost = %.2f \n",path_cost);
00418 
00419         delete[] my_tasks;
00420 
00421         return path_cost;
00422 }
00423 
00424 void SSIPatrolAgent::force_bid(int nv,double bv,int rid){
00425         //printf("forcing bid for vertex %d with value %.2f from robot %d \n",nv,bv,rid);
00426         bids[nv].bidValue = bv;
00427         bids[nv].robotId = rid;
00428 }
00429 
00430 void SSIPatrolAgent::wait(){
00431         double t = std::min(timeout,1.0+nactivetasks*0.1);
00432 #if DEBUG_PRINT
00433         printf("   --- waiting %.1f second ---\n",t);
00434 #endif
00435         ros::Duration delay = ros::Duration(t); //asynchronous version
00436         delay.sleep();  
00437 
00438 /*        double micro_timeout = timeout/ts; //synchronous version
00439         for (int i=0;i<ts;i++){
00440                 // ros::spinOnce();
00441                 ros::Duration delay = ros::Duration(micro_timeout);
00442                 delay.sleep();  
00443         }
00444 */
00445 }
00446 
00447 
00448 int SSIPatrolAgent::compute_next_vertex() {
00449         compute_next_vertex(current_vertex);
00450 }
00451 
00452 // current_vertex (goal just reached)
00453 int SSIPatrolAgent::compute_next_vertex(int cv) {
00454 
00455     update_global_idleness();
00456 
00457 
00458         //consider all possible vertices as next target (i.e., set all vertices to false)       
00459     reset_selected_vertices(selected_vertices);
00460 
00461         //consider only neighbouring vertices for next_vertex selection (i.e., set to false only neighbouring vertices)
00462         //NOTE: if none of the neighbouring vertices is assigned to the agent all other vertices will be considered (see reset_selected_vertices() called in select_next_vertex(...))
00463         //select_faraway_vertices(selected_vertices,cv);        
00464     if (cv >= 0 && cv < dimension){
00465         selected_vertices[cv] = true; //do not consider current vertex as possible goal 
00466     }
00467 
00468     int mnv = select_next_vertex(cv,selected_vertices); 
00469     double bidvalue = compute_bid(mnv); 
00470     int value = ID_ROBOT;
00471     if (value==-1){value=0;}
00472     force_bid(mnv,bidvalue,value); 
00473     send_target(mnv,bidvalue);
00474 #if DEBUG_PRINT    
00475     printf("DTAP [%.1f] compute_next_vertex: waiting for bids\n",ros::Time::now().toSec());
00476 #endif
00477     wait();
00478 #if DEBUG_PRINT    
00479     printf("DTAP compute_next_vertex: bids timeout - current value for target node %d = %.2f \n",mnv,bidvalue);
00480 #endif
00481 
00482     /*
00483     printf("Tasks [");
00484     for (size_t i = 0; i<dimension;i++){
00485                 printf(" %d, ",tasks[i]);       
00486     }
00487     printf("] \n");
00488     */
00489     
00490     
00491     //printf("DTAP: while(true) ... \n");
00492     while (true){
00493 
00494         if (best_bid(mnv)){ //if I am in the best position to go to mnv 
00495                         update_tasks();
00496                         //force_bid(mnv,0,value); TODO: check
00497                         break;
00498         } else {
00499                 if (greedy_best_bid(cv,mnv)){ //if the greedy action condition is true stop the vertex selection and go to mvn (do not update your task)
00500 #if DEBUG_PRINT
00501                         //get date and time for file name
00502             time_t rawtime;
00503                     struct tm * timeinfo;
00504                         char strnow[80];
00505                         time (&rawtime);
00506                         timeinfo = localtime(&rawtime);
00507                         sprintf(strnow,"%d%02d%02d_%02d%02d%02d",  timeinfo->tm_year+1900,timeinfo->tm_mon+1,timeinfo->tm_mday,timeinfo->tm_hour,timeinfo->tm_min,timeinfo->tm_sec);
00508                         //open file
00509                         FILE* fp = fopen("greedy-actions.txt","a");
00510                         fprintf(fp,"time: %s; robot: %d; target vertex: %d; current vertex: %d\n",strnow,value,mnv,cv);
00511                         fclose(fp);
00512 #endif
00513                         //exit from while loop  
00514                         break;
00515                 } else {
00516                         mnv = select_next_vertex(cv,selected_vertices); 
00517                         bidvalue = compute_bid(mnv); 
00518                         force_bid(mnv,bidvalue,value); 
00519                         send_target(mnv,bidvalue);
00520                         //printf("  ... waiting for bids (%.2f seconds) ... \n",timeout);
00521                         wait();
00522                         /*printf("current target %d current value for target %.2f tasks [",mnv,bidvalue);
00523                             for (size_t i = 0; i<dimension;i++){
00524                                         printf(" %d, ",tasks[i]);       
00525                             }
00526                             printf("] \n");*/                   
00527                 }
00528         }                       
00529     }
00530     //printf("DTAP: while(true) ... DONE\n");
00531     
00532     return mnv;
00533     
00534 }
00535 
00536 
00537 // NOTE: redefined in DTASSIPart_Agent
00538 void SSIPatrolAgent::update_tasks(){
00539     int value = ID_ROBOT;
00540     if (value==-1){value=0;}
00541     for (size_t i = 0; i< dimension; i++){
00542         tasks[i] = (bids[i].robotId == value);
00543     }
00544 }
00545 
00546 void SSIPatrolAgent::send_target(int nv,double bv) {
00547         //msg format: [ID_ROBOT,msg_type,next_vertex_index,bid_value]
00548         int value = ID_ROBOT;
00549         if (value==-1){value=0;}
00550     
00551         int msg_type = DTASSI_TR;
00552         std_msgs::Int16MultiArray msg;
00553         msg.data.clear();
00554 
00555         msg.data.push_back(value);
00556         msg.data.push_back(msg_type);
00557         msg.data.push_back(nv);
00558 #if DEBUG_PRINT
00559         printf("DTAP [%.1f]  ** sending Task Request [robot:%d, msgtype:%d, next_vertex:%d, bid:%.2f ] \n",
00560                 ros::Time::now().toSec(),value,msg_type,nv,bv);
00561 #endif
00562         int ibv = (int)(bv);
00563         if (ibv>32767) { // Int16 is used to send messages
00564             ROS_WARN("Wrong conversion when sending bid value in messages!!!");
00565             ibv=32000;
00566         }
00567         msg.data.push_back(ibv);
00568         
00569         do_send_message(msg);   
00570     
00571 }
00572 
00573 
00574 void SSIPatrolAgent::send_bid(int nv,double bv) {
00575         int value = ID_ROBOT;
00576         if (value==-1){value=0;}
00577         
00578         //msg format: [ID_ROBOT,msg_type,next_vertex_index,bid_value]
00579         int msg_type = DTASSI_BID;
00580         std_msgs::Int16MultiArray msg;
00581         msg.data.clear();
00582 
00583         msg.data.push_back(value);
00584         msg.data.push_back(msg_type);
00585         msg.data.push_back(nv);
00586 #if DEBUG_PRINT
00587         printf("DTAP  ** sending Bid [robot:%d, msgtype:%d, next_vertex:%d, bid:%.2f ] \n",value,msg_type,nv,bv);
00588 #endif
00589         int ibv = (int)(bv);
00590         if (ibv>32767) { // Int16 is used to send messages
00591             ROS_WARN("Wrong conversion when sending bid value in messages!!!");
00592             ibv=32000;
00593         }
00594         msg.data.push_back(ibv);
00595         do_send_message(msg);   
00596 }
00597 
00598 
00599 //return true if the robot holds the best bid for nv OR if the vertex is adjacent on the patrol graph, the idleness is much higher than normal and no one else is going to the same vertex
00600 bool SSIPatrolAgent::greedy_best_bid(int cv, int nv){
00601 
00602 
00603 //      bool my_best = (bids[nv].robotId==ID_ROBOT);
00604 //      printf("CHECK BEST: bid robot id %d, result: %d \n",bids[nv].robotId,(bids[nv].robotId==ID_ROBOT));
00605 
00606 
00607         bool adj = (compute_hops(cv,nv) <= 1);
00608 
00609         double avg_idleness = 0.;
00610         for(size_t i=0; i<dimension; i++) {
00611                 avg_idleness += global_instantaneous_idleness[i];
00612         }
00613         avg_idleness = avg_idleness/((double) dimension);
00614         double std_idleness = 0.;
00615         for(size_t i=0; i<dimension; i++) {
00616         std_idleness += (global_instantaneous_idleness[i] - avg_idleness)*(global_instantaneous_idleness[i] - avg_idleness);
00617         }
00618         std_idleness = sqrt(std_idleness/((double) dimension));
00619         bool high_idleness = (global_instantaneous_idleness[nv] > (2*std_idleness + avg_idleness));
00620         bool conflict = (bids[nv].bidValue == 0);
00621         bool greedy_cond = adj && high_idleness && !conflict;
00622 
00623 #if DEBUG_PRINT
00624         printf("CHECK ADJ: cv %d, nv %d, hops: %d, result: %d \n",cv,nv,compute_hops(cv,nv),(int)adj);
00625         printf("CHECK HIGH IDLNESS: idl %f, avg %f, std %f, result: %d \n",global_instantaneous_idleness[nv],avg_idleness,std_idleness,high_idleness);
00626         printf("CHECK CONF: bid value %f, result: %d \n",bids[nv].bidValue,conflict);
00627         printf("CHECK GREEDY: result: %d \n",greedy_cond);
00628 #endif
00629 
00630 //      return my_best || greedy_cond;  
00631         return greedy_cond;
00632 }
00633 
00634 
00635 //return true if I have this robot holds the lowest bid for nv
00636 bool SSIPatrolAgent::best_bid(int nv){
00637         // printf("computing whether I hold the best bid for %d, result: %d \n",nv,(bids[nv].robotId==ID_ROBOT));
00638 
00639 /*      printf("bids [");
00640         for (size_t i = 0; i<dimension; i++){
00641             printf(" <%.2f,%d>, ",bids[i].bidValue,bids[i].robotId);    
00642         }
00643         printf("] \n"); 
00644 */
00645         int value = ID_ROBOT;
00646         if (value==-1){value=0;}
00647         return (bids[nv].robotId==value);       
00648 }
00649 
00650 // current_vertex (goal just reached)
00651 // next_vertex (next goal)
00652 //make this blocking to wait for bids
00653 //
00654 void SSIPatrolAgent::send_results() {
00655     int value = ID_ROBOT;
00656     if (value==-1){value=0;}
00657     
00658     //result= [ID,msg_type,global_idleness[1..dimension],next_vertex]
00659     int msg_type = DTAGREEDY_MSG_TYPE;
00660     std_msgs::Int16MultiArray msg;
00661     msg.data.clear();
00662     msg.data.push_back(value);
00663     msg.data.push_back(msg_type);
00664     // printf("  ** sending [%d, %d, ",ID_ROBOT,msg_type);
00665     pthread_mutex_lock(&lock);
00666     for(size_t i=0; i<dimension; i++) {
00667         // convert in 1/10 of secs (integer value) Max value 3276.8 second (> 50 minutes) !!!
00668         int ms = (int)(global_instantaneous_idleness[i]*10);
00669         if (ms>32767) { // Int16 is used to send messages
00670             ROS_WARN("Wrong conversion when sending idleness value in messages!!!");
00671             printf("*** idleness value = %.1f -> int16 value = %d\n",global_instantaneous_idleness[i],ms);
00672             ms=32000;
00673         }
00674 //        if ((int)i==next_vertex) ms=0; //sending 0 for next vertex to avoid conflicts (useless for DTASSI) TODO:CHECK 
00675         //printf("  ** sending GII[%lu] = %d\n",i,ms);
00676         //printf("%d, ",ms);
00677         msg.data.push_back(ms);
00678     }
00679     pthread_mutex_unlock(&lock);
00680     msg.data.push_back(next_vertex);
00681     //printf("%d]\n",next_vertex);
00682     
00683     do_send_message(msg);   
00684 }
00685 
00686 void SSIPatrolAgent::update_bids(int nv, double bv, int senderId){
00687         if (bids[nv].bidValue >= (1 + hist)*bv) { //using histeresis to avoid switching when there is no clear benefit
00688                 bids[nv].bidValue = bv;
00689                 bids[nv].robotId = senderId;
00690         }
00691         update_tasks();
00692 }
00693 
00694 void SSIPatrolAgent::idleness_msg_handler(std::vector<int>::const_iterator it){
00695 
00696     double now = ros::Time::now().toSec();
00697     pthread_mutex_lock(&lock);
00698     for(size_t i=0; i<dimension; i++) {
00699                 int ms = *it; it++; // received value
00700                 // printf("  ** received from %d remote-GII[%lu] = %.1f\n",id_sender,i,ms);
00701                 //printf("%d, ",ms);
00702                 double rgi = (double)ms/10.0; // convert back in seconds
00703                 global_instantaneous_idleness[i] = std::min(
00704                         global_instantaneous_idleness[i]+(now-last_update_idl), rgi);
00705                 // printf("   ++ GII[%lu] = %.1f (r=%.1f)\n",i,global_instantaneous_idleness[i],rgi);
00706     }
00707     pthread_mutex_unlock(&lock);
00708     last_update_idl = now;
00709 
00710 /* TODO: CONSIDER INCLUDING THIS IN SSI AS A DIRECT DE-CONFLICTING PROCEDURE
00711 
00712                     int sender_next_vertex = *it; it++;
00713                     //printf("%d]\n",sender_next_vertex);
00714                     
00715                     // interrupt path if moving to the same target node
00716                     if (sender_next_vertex == next_vertex) { // two robots are going to the same node
00717                         ROS_INFO("Robots %d and %d are both going to vertex %d",ID_ROBOT,id_sender,next_vertex);
00718                         ROS_INFO("Robot %d: STOP and choose another target",ID_ROBOT);
00719                         // change my destination
00720                         cancelGoal(); // stop the current behavior
00721                         current_vertex = next_vertex; // simulate that the goal vertex has been reached (not sent to the monitor)
00722                         next_vertex = compute_next_vertex(); // compute next vertex (will be different from current vertex)
00723                         sendGoal(next_vertex);
00724                     }
00725 */
00726 }
00727 
00728 #if 0
00729 void SSIPatrolAgent::task_request_msg_handler(std::vector<int>::const_iterator it, int senderId){
00730         int nv = *it; it++;
00731         double bv = *it; it++;
00732 #if DEBUG_PRINT
00733         printf("DTAP handling task request message form %d: [ vertex: %d, bid value: %.2f]\n",senderId,nv,bv);
00734 #endif
00735         double now = ros::Time::now().toSec();
00736         taskRequests[nv] = now;
00737         double my_bidValue = compute_bid(nv); 
00738         update_bids(nv,bv,senderId);
00739         if (my_bidValue<bv*(1+hist)){
00740                 send_bid(nv,my_bidValue);
00741         }
00742 }
00743 #else
00744 void SSIPatrolAgent::task_request_msg_handler(std::vector<int>::const_iterator it, int senderId){
00745         int nv = *it; it++;
00746         double bv = *it; it++;
00747         double now = ros::Time::now().toSec();
00748 #if DEBUG_PRINT
00749         printf("DTAP [%.1f] handling task request message form %d: [ vertex: %d, bid value: %.2f]\n",now,senderId,nv,bv);
00750 #endif
00751 
00752         taskRequests[nv] = now;
00753 
00754         update_bids(nv,bv,senderId);  // update bids with sender value
00755 
00756 // CHECK: added Luca !!!
00757         //if (nv==next_vertex || nv==next_next_vertex) {
00758             double my_bidValue = compute_bid(nv);
00759             int value = ID_ROBOT;
00760             if (value==-1){value=0;}
00761             update_bids(nv,my_bidValue,value);  // update bids with my value
00762         //}
00763 
00764 //      if (my_bidValue<bv*(1+hist)){
00765         if (bids[nv].robotId==value){
00766 //              send_bid(nv,my_bidValue);
00767                 send_bid(nv,bids[nv].bidValue);
00768         }
00769 }
00770 #endif
00771 
00772 
00773 void SSIPatrolAgent::bid_msg_handler(std::vector<int>::const_iterator it, int senderId){
00774         int nv = *it; it++;
00775         double bv = *it; it++;
00776 #if DEBUG_PRINT
00777         printf("DTAP [%.1f] handling bid message from %d: [ vertex: %d, bid value: %.2f]\n",ros::Time::now().toSec(),senderId,nv,bv);
00778 #endif
00779         update_bids(nv,bv,senderId);
00780 }
00781 
00782 
00783 void SSIPatrolAgent::receive_results() {
00784     //result= [ID,msg_type,global_idleness[1..dimension],next_vertex]
00785         
00786     std::vector<int>::const_iterator it = vresults.begin();
00787     int id_sender = *it; it++;
00788     int value = ID_ROBOT;
00789     if (value==-1){value=0;}
00790     if (id_sender==value) return;
00791     int msg_type = *it; it++;
00792     //printf("  ** received [%d, %d, ... ] \n",id_sender,msg_type);
00793     switch (msg_type){
00794             case (DTAGREEDY_MSG_TYPE):
00795             {
00796                 idleness_msg_handler(it);
00797             }
00798             break;
00799             case (DTASSI_TR):
00800             {
00801                 task_request_msg_handler(it,id_sender);
00802             }   
00803             break;                          
00804             case (DTASSI_BID):
00805             {
00806                 bid_msg_handler(it,id_sender);
00807             }   
00808             break;
00809    }
00810 }
00811 
00812 
00813 
00814 


patrolling_sim
Author(s):
autogenerated on Thu Jun 6 2019 20:27:09