Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
00118 void GBS_Agent::processEvents() {
00119
00120 if (arrived && NUMBER_OF_ROBOTS>1){
00121
00122
00123 double now = ros::Time::now().toSec();
00124
00125 for(int i=0; i<dimension; i++){
00126 if (i == vertex_arrived){
00127
00128 last_visit[vertex_arrived] = now;
00129
00130 }
00131
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
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 }