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


patrolling_sim
Author(s):
autogenerated on Mon Oct 2 2017 03:13:50