SEBS_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 SEBS_Agent: public PatrolAgent {
00054 
00055 private:
00056 
00057   double G1, G2;
00058   double edge_min;  
00059   int NUMBER_OF_ROBOTS;
00060   int *tab_intention;
00061   bool arrived;
00062   uint vertex_arrived;
00063   int robot_arrived;
00064   bool intention;
00065   uint vertex_intention;
00066   int robot_intention;  
00067       
00068 public:
00069     virtual void init(int argc, char** argv);
00070     virtual int compute_next_vertex();
00071     virtual void processEvents();
00072     virtual void send_results();
00073     virtual void receive_results();    
00074 };
00075 
00076 
00077 void SEBS_Agent::init(int argc, char** argv) {
00078   
00079   PatrolAgent::init(argc,argv);
00080   
00081   NUMBER_OF_ROBOTS = atoi(argv[3]);
00082   arrived=false;
00083   intention=false;
00084   
00086   G1 = 0.1;
00087  
00088   //default:
00089   G2 = 100.0;
00090   edge_min = 1.0;
00091 
00092 #if 0
00093   if (graph_file=="maps/grid/grid.graph") {  
00094     if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;}
00095     if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;}
00096     if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;}
00097     if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;}
00098     if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;}
00099     if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;}
00100     
00101   }else if (graph_file=="maps/example/example.graph") {
00102     if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;}
00103     if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;}
00104     if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;}
00105     if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;}
00106     if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;}
00107     edge_min = 20.0;
00108     
00109   }else if (graph_file=="maps/cumberland/cumberland.graph") {
00110     if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;}
00111     if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;}
00112     if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;}
00113     if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;}
00114     if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;}    
00115     edge_min = 50.0;    
00116   }
00117 #endif
00118 
00119   printf("G1 = %f, G2 = %f\n", G1, G2); 
00120   
00121     std::stringstream paramss;
00122     paramss << G1 << "," << G2;
00123 
00124     ros::param::set("/algorithm_params",paramss.str());
00125 
00126     
00127   //INITIALIZE tab_intention:
00128   tab_intention = new int[NUMBER_OF_ROBOTS];
00129   for (int i=0; i<NUMBER_OF_ROBOTS; i++){
00130     tab_intention[i] = -1;
00131   }
00132   
00133 }
00134 
00135 // Executed at any cycle when goal is not reached
00136 void SEBS_Agent::processEvents() {
00137     
00138     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.
00139 
00140         //ROS_INFO("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);    
00141 
00142         //Update Idleness Table:
00143         double now = ros::Time::now().toSec();
00144                 
00145         for(int i=0; i<dimension; i++){
00146             if (i == vertex_arrived){
00147                 //actualizar last_visit[dimension]
00148                 last_visit[vertex_arrived] = now;   
00149             }           
00150             //actualizar instantaneous_idleness[dimension]
00151             instantaneous_idleness[i] = now - last_visit[i];      
00152             //ROS_INFO("idleness[%d] = %f", i, instantaneous_idleness[i]);
00153         }     
00154         
00155         arrived = false;
00156     }
00157 
00158     if (intention && NUMBER_OF_ROBOTS>1) {    
00159         tab_intention[robot_intention] = vertex_intention;
00160         //printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
00161         intention = false;
00162     }
00163     // ros::spinOnce();    
00164 }
00165 
00166 int SEBS_Agent::compute_next_vertex() {
00167     return state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min);
00168 }
00169 
00170 
00171 void SEBS_Agent::send_results() {   
00172     int value = ID_ROBOT;
00173     if (value==-1){value=0;}
00174     // [ID,msg_type,vertex,intention]
00175     std_msgs::Int16MultiArray msg;   
00176     msg.data.clear();
00177     msg.data.push_back(value);
00178     msg.data.push_back(SEBS_MSG_TYPE);
00179     msg.data.push_back(current_vertex);
00180     msg.data.push_back(next_vertex);    
00181     do_send_message(msg);
00182 }
00183 
00184 void SEBS_Agent::receive_results() {
00185   
00186     std::vector<int>::const_iterator it = vresults.begin();
00187     int id_sender = *it; it++;
00188     int msg_type = *it; it++;
00189     
00190     int value = ID_ROBOT;
00191     if (value==-1){value=0;}
00192     
00193         if ((id_sender==value) || (msg_type!=SEBS_MSG_TYPE)) 
00194         return;
00195         
00196     robot_arrived = vresults[0];
00197     vertex_arrived = vresults[2];
00198     arrived = true;
00199     robot_intention = vresults[0];
00200     vertex_intention = vresults[3];
00201     intention = true;
00202 }
00203 
00204 int main(int argc, char** argv) {
00205 
00206     SEBS_Agent agent;
00207     agent.init(argc,argv);    
00208     agent.run();
00209 
00210     return 0; 
00211 }
00212 
00213 


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