CBLS_Agent.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: David Portugal (2011-2014), and Luca Iocchi (2014-2016)
00036 *********************************************************************/
00037 
00038 #include <sstream>
00039 #include <ros/ros.h>
00040 #include <move_base_msgs/MoveBaseAction.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <tf/transform_broadcaster.h>
00043 #include <tf/transform_listener.h>
00044 #include <nav_msgs/Odometry.h>
00045 
00046 #include "PatrolAgent.h"
00047 #include "getgraph.h"
00048 #include "algorithms.h"
00049 
00050 
00051 using namespace std;
00052 
00053 class CBLS_Agent: public PatrolAgent {
00054 
00055 private:
00056 
00057   int NUMBER_OF_ROBOTS;
00058   int *tab_intention;
00059   uint *node_count;
00060   bool arrived;
00061   uint vertex_arrived;
00062   int robot_arrived;
00063   bool intention;
00064   uint vertex_intention;
00065   int robot_intention;  
00066   int number_of_edges;
00067   reinforcement_learning RL;
00068   long long int decision_number;
00069   double now;
00070   double *avg_idleness;  // local idleness
00071   double *cur_avg_idleness;  // local idleness
00072   double *real_histogram; 
00073   double *histogram;
00074   uint *source;
00075   uint *destination;
00076   uint hist_dimension;
00077       
00078 public:
00079     virtual void init(int argc, char** argv);
00080     virtual int compute_next_vertex();
00081     virtual void processEvents();
00082     virtual void send_results();
00083     virtual void receive_results();    
00084     virtual void onGoalComplete();    
00085 };
00086 
00087 
00088 void CBLS_Agent::init(int argc, char** argv) {
00089    
00090   PatrolAgent::init(argc,argv);
00091   
00092   NUMBER_OF_ROBOTS = atoi(argv[3]);
00093   uint number_of_edges = GetNumberEdges(vertex_web, dimension);  
00094     
00095   //INITIALIZE tab_intention:
00096   int i;
00097   
00098   tab_intention = new int[NUMBER_OF_ROBOTS];
00099   for (i=0; i<NUMBER_OF_ROBOTS; i++){
00100     tab_intention[i] = -1;
00101   }
00102   
00103   //INITIALIZE node_count:
00104   node_count = new uint[dimension];
00105   for (i=0; i<dimension; i++){
00106     node_count[i] = 0;
00107   }  
00108   
00109   hist_dimension = 2*number_of_edges; //directed edges, i.e.: arcs
00110   
00111   source = new uint [hist_dimension];
00112   destination = new uint [hist_dimension];
00113 
00114   create_source_and_dest_tables(vertex_web, source, destination, dimension);
00115   
00116   real_histogram = new double[hist_dimension];
00117   histogram = new double[hist_dimension];        // between 0 and 1
00118 
00119   for (i=0; i<hist_dimension; i++){
00120     real_histogram[i] = 1.0;
00121     histogram[i] = 1.0 / (double) hist_dimension;
00122   } 
00123   
00124   //INITIALIZE tables:
00125   double time_zero = ros::Time::now().toSec();  
00126 
00127   avg_idleness = new double[dimension]; //closed avg
00128   cur_avg_idleness = new double[dimension];     //avg + inst
00129    
00130   for(i=0;i<dimension;i++){ 
00131     instantaneous_idleness[i]= 0.0;
00132     avg_idleness[i] = 0.0;
00133     cur_avg_idleness[i]=0.0;
00134     last_visit[i]= time_zero; 
00135     
00136     if(i==current_vertex){
00137       last_visit[i]= time_zero + 0.1; //Avoids getting back immediately at the initial vertex
00138       node_count[i]= 1;
00139     }
00140   }
00141   
00142   decision_number = 0;  
00143   
00144 }
00145 
00146 void CBLS_Agent::onGoalComplete() {
00147   
00148     if( (next_vertex>-1) && (current_vertex != next_vertex) ){
00149       
00151                 //Update Idleness Table:
00152                 now = ros::Time::now().toSec();
00153                         
00154                 for(int i=0; i<dimension; i++){
00155                         if (i == next_vertex){
00156                                 last_visit[i] = now;
00157                                 node_count[i]++;
00158                                 avg_idleness[i] = ( avg_idleness[i] * (double) (node_count [i] - 1) + instantaneous_idleness [i] ) / ( (double) node_count [i] );                                   
00159                         }       
00160                         instantaneous_idleness[i]= now - last_visit[i];  //ou seja: Zero para o next_vertex                     
00161                         //ROS_INFO("inst_idleness[%d] = %f", i, instantaneous_idleness[i]);
00162                         
00163                         //Update Curr Avg Idleness Table:
00164                         cur_avg_idleness [i] = ( avg_idleness [i] * (double) (node_count [i]) + instantaneous_idleness [i] ) / ( (double) node_count [i] + 1 );
00165                 }
00166                 
00168                 int value = ID_ROBOT;
00169                 if (value==-1){value=0;}
00170                 
00171                 update_likelihood_new(RL, node_count, instantaneous_idleness, dimension, real_histogram, source, destination, hist_dimension, vertex_web, value);
00172                 normalize_histogram(real_histogram, histogram, hist_dimension); 
00173                   
00174                 /*if(decision_number%50 == 0){ //write histogram each 50 iterations 
00175                   write_histogram_to_file (vertex_web, real_histogram, histogram, source, destination, hist_dimension,decision_number,value);
00176                 }*/
00177                   
00178                 decision_number++;
00179                 //ROS_INFO("decision_number = %lld", decision_number);          
00180                   
00181                 current_vertex = next_vertex;             
00182     }
00183     
00185         next_vertex = compute_next_vertex();
00186         
00187         if(next_vertex == -1){
00188          ROS_ERROR("ABORT (learning_algorithm: next_vertex = -1)");
00189          exit(-1);
00190         }       
00197          //if (current_vertex==next_vertex){
00198          //  goal_complete = true; //do not try to go there!
00199          //  //Stay in the same place to avoid interference
00200            
00201          //}else{
00202             
00203           //send_info(current_vertex, next_vertex);
00204           send_goal_reached(); // Send TARGET to monitor
00205           send_results();  // Algorithm specific function
00206           
00207           ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
00208           //sendGoal(vertex_web[next_vertex].x, vertex_web[next_vertex].y);  
00209           sendGoal(next_vertex);  // send to move_base
00210           
00211           goal_complete = false;          
00212          //}    
00213   
00214 }
00215 
00216 // Executed at any cycle when goal is not reached
00217 void CBLS_Agent::processEvents() {
00218     
00219     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.
00220 
00221         //ROS_INFO("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);    
00222 
00223         //Update Idleness Table:
00224         now = ros::Time::now().toSec();
00225                 
00226         for(int i=0; i<dimension; i++){
00227             if (i == vertex_arrived){
00228                 //actualizar last_visit[dimension]
00229                 last_visit[vertex_arrived] = now;   
00230                           node_count[vertex_arrived]++;
00231                           avg_idleness[i] = ( avg_idleness[i] * (double) (node_count [i] - 1) + instantaneous_idleness [i] ) / ( (double) node_count [i] );             
00232             }           
00233             //actualizar instantaneous_idleness[dimension]
00234             instantaneous_idleness[i] = now - last_visit[i];      
00235             cur_avg_idleness [i] = ( avg_idleness [i] * (double) (node_count [i]) + instantaneous_idleness [i] ) / ( (double) node_count [i] + 1 );                                                 
00236             //ROS_INFO("idleness[%d] = %f", i, instantaneous_idleness[i]);
00237         }     
00238         
00239         arrived = false;
00240     }
00241 
00242     if (intention && NUMBER_OF_ROBOTS>1) {    
00243         tab_intention[robot_intention] = vertex_intention;
00244         //printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
00245         intention = false;
00246     }
00247     // ros::spinOnce();   
00248 
00249 }
00250 
00251 int CBLS_Agent::compute_next_vertex() {
00252     int value = ID_ROBOT;
00253     if (value==-1){value=0;}
00254     return learning_algorithm (current_vertex, vertex_web, instantaneous_idleness, cur_avg_idleness, tab_intention, histogram, source, destination, hist_dimension, TEAMSIZE, value, node_count, RL);
00255 }
00256 
00257 
00258 void CBLS_Agent::send_results() {  
00259     int value = ID_ROBOT;
00260     if (value==-1){value=0;}
00261     // [ID,msg_type,vertex,intention]
00262     std_msgs::Int16MultiArray msg;   
00263     msg.data.clear();
00264     msg.data.push_back(value);
00265     msg.data.push_back(CBLS_MSG_TYPE);
00266     msg.data.push_back(current_vertex);
00267     msg.data.push_back(next_vertex);    
00268     do_send_message(msg);
00269 }
00270 
00271 void CBLS_Agent::receive_results() {
00272   
00273     std::vector<int>::const_iterator it = vresults.begin();
00274     int id_sender = *it; it++;
00275     int msg_type = *it; it++;
00276     
00277     int value = ID_ROBOT;
00278     if (value==-1){value=0;}
00279     
00280         if ((id_sender==value) || (msg_type!=CBLS_MSG_TYPE)) 
00281         return;
00282         
00283     robot_arrived = vresults[0];
00284     vertex_arrived = vresults[2];
00285     arrived = true;
00286     robot_intention = vresults[0];
00287     vertex_intention = vresults[3];
00288     intention = true;
00289 }
00290 
00291 int main(int argc, char** argv) {
00292 
00293     CBLS_Agent agent;
00294     agent.init(argc,argv);    
00295     agent.run();
00296 
00297     return 0; 
00298 }
00299 
00300 


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