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 <string>
00040 #include <pthread.h>
00041 #include <ros/ros.h>
00042 #include <move_base_msgs/MoveBaseAction.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <tf/transform_broadcaster.h>
00045 #include <tf/transform_listener.h>
00046 #include <nav_msgs/Odometry.h>
00047 #include <std_msgs/Int16MultiArray.h>
00048
00049 #include "PatrolAgent.h"
00050 #include "algorithms.h"
00051 #include "config.h"
00052
00053 #define CONFIG_FILENAME "params/DTA/DTAGreedy.params"
00054
00055 class DTAGreedy_Agent: public PatrolAgent {
00056 private:
00057 double *global_instantaneous_idleness;
00058 double last_update_idl;
00059 ConfigFile cf;
00060 double theta_idl, theta_cost, theta_odist;
00061 float origin_x, origin_y, origin_theta;
00062 pthread_mutex_t lock;
00063
00064 public:
00065 DTAGreedy_Agent() : cf(CONFIG_FILENAME)
00066 {
00067 pthread_mutex_init(&lock, NULL);
00068
00069 }
00070
00071 virtual void init(int argc, char** argv);
00072 virtual int compute_next_vertex();
00073 virtual void send_results();
00074 virtual void receive_results();
00075
00076 double compute_cost(int vertex);
00077 double distanceFromOrigin(int vertex);
00078 double utility(int vertex);
00079 void update_global_idleness();
00080 };
00081
00082 void DTAGreedy_Agent::init(int argc, char** argv) {
00083
00084 printf("DTAGreedy_Agent::init\n");
00085
00086 PatrolAgent::init(argc,argv);
00087
00088 global_instantaneous_idleness = new double[dimension];
00089 for(size_t i=0; i<dimension; i++) {
00090 global_instantaneous_idleness[i]=100;
00091 }
00092
00093 last_update_idl = ros::Time::now().toSec();
00094
00095 theta_idl = cf.getDParam("theta_idleness");
00096 theta_cost = cf.getDParam("theta_navigation");
00097 theta_odist = cf.getDParam("theta_distance_from_origin");
00098
00099 std::stringstream paramss;
00100 paramss << theta_idl << "," << theta_cost << "," << theta_odist;
00101
00102 ros::param::set("/algorithm_params",paramss.str());
00103
00104 int value = ID_ROBOT;
00105 if (value==-1){value=0;}
00106 getRobotPose(value,origin_x, origin_y, origin_theta);
00107 ROS_INFO("Robot %d: Initial pose %.1f %.1f %.1f",value,origin_x, origin_y, origin_theta);
00108
00109 }
00110
00111 double DTAGreedy_Agent::compute_cost(int vertex)
00112 {
00113 uint elem_s_path;
00114 int *shortest_path = new int[dimension];
00115 int id_neigh;
00116
00117 dijkstra( current_vertex, vertex, shortest_path, elem_s_path, vertex_web, dimension);
00118 double distance = 0;
00119
00120 for(uint j=0; j<elem_s_path; j++){
00121 if (j<elem_s_path-1){
00122 id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00123 distance += vertex_web[shortest_path[j]].cost[id_neigh];
00124 }
00125 }
00126
00127 return distance;
00128 }
00129
00130
00131 double DTAGreedy_Agent::distanceFromOrigin(int vertex) {
00132 double x = vertex_web[vertex].x, y = vertex_web[vertex].y;
00133 return sqrt((origin_x-x)*(origin_x-x)+(origin_y-y)*(origin_y-y));
00134 }
00135
00136 double DTAGreedy_Agent::utility(int vertex) {
00137
00138 double idl = global_instantaneous_idleness[vertex];
00139 double cost = compute_cost(vertex);
00140 double odist = distanceFromOrigin(vertex);
00141 double U = theta_idl * idl + theta_cost * cost + theta_odist * odist;
00142 printf(" -- U[%d] ( %.1f, %.1f, %.1f ) = ( %.1f, %.1f, %.1f ) = %.1f\n",vertex,idl,cost,odist,theta_idl*idl,theta_cost*cost,theta_odist*odist,U);
00143 return U;
00144 }
00145
00146 void DTAGreedy_Agent::update_global_idleness()
00147 {
00148 double now = ros::Time::now().toSec();
00149
00150 pthread_mutex_lock(&lock);
00151 for(size_t i=0; i<dimension; i++) {
00152 global_instantaneous_idleness[i] += (now-last_update_idl);
00153 }
00154 pthread_mutex_unlock(&lock);
00155
00156 last_update_idl = now;
00157 }
00158
00159
00160 int DTAGreedy_Agent::compute_next_vertex() {
00161
00162 update_global_idleness();
00163 global_instantaneous_idleness[current_vertex] = 0.0;
00164
00165
00166 double maxUtility = -1e9;
00167 int i_maxUtility = 0;
00168
00169 for(size_t i=0; i<dimension; i++){
00170
00171 double U = utility(i);
00172 if (U > maxUtility && i!=current_vertex){
00173 maxUtility = U;
00174 i_maxUtility = i;
00175 }
00176
00177 }
00178
00179 int nv = i_maxUtility;
00180
00181 printf("DTAGreedy: next vertex = %d (U = %.2f)\n",nv,maxUtility);
00182
00183 return nv;
00184 }
00185
00186
00187
00188
00189
00190 void DTAGreedy_Agent::send_results() {
00191 int value = ID_ROBOT;
00192 if (value==-1){value=0;}
00193
00194
00195 int msg_type = DTAGREEDY_MSG_TYPE;
00196 std_msgs::Int16MultiArray msg;
00197 msg.data.clear();
00198 msg.data.push_back(value);
00199 msg.data.push_back(msg_type);
00200
00201
00202 pthread_mutex_lock(&lock);
00203
00204 for(size_t i=0; i<dimension; i++) {
00205
00206 int ms = (int)(global_instantaneous_idleness[i]*10);
00207 if (ms>32768) {
00208 ROS_WARN("Wrong conversion when sending idleness value in messages!!!");
00209 ms=32000;
00210 }
00211 if ((int)i==next_vertex) ms=0;
00212
00213
00214 msg.data.push_back(ms);
00215 }
00216 msg.data.push_back(next_vertex);
00217
00218 pthread_mutex_unlock(&lock);
00219
00220 do_send_message(msg);
00221
00222 }
00223
00224
00225 void DTAGreedy_Agent::receive_results() {
00226
00227
00228 double now = ros::Time::now().toSec();
00229
00230
00231
00232 std::vector<int>::const_iterator it = vresults.begin();
00233 int id_sender = *it; it++;
00234 int msg_type = *it; it++;
00235
00236
00237 int value = ID_ROBOT;
00238 if (value==-1){value=0;}
00239
00240 if ((id_sender==value) || (msg_type!=DTAGREEDY_MSG_TYPE))
00241 return;
00242 pthread_mutex_lock(&lock);
00243 for(size_t i=0; i<dimension; i++) {
00244 int ms = *it; it++;
00245
00246
00247 double rgi = (double)ms/10.0;
00248
00249
00250 if (isnan(global_instantaneous_idleness[i])) {
00251 printf("NAN Exiting!!!"); return;
00252 }
00253
00254 global_instantaneous_idleness[i] = std::min(
00255 global_instantaneous_idleness[i]+(now-last_update_idl), rgi);
00256
00257 }
00258 pthread_mutex_unlock(&lock);
00259 last_update_idl = now;
00260
00261 int sender_next_vertex = *it; it++;
00262
00263
00264
00265 if (sender_next_vertex == next_vertex) {
00266 ROS_INFO("Robots %d and %d are both going to vertex %d",value,id_sender,next_vertex);
00267 ROS_INFO("Robot %d: STOP and choose another target",value);
00268
00269 cancelGoal();
00270 current_vertex = next_vertex;
00271 next_vertex = compute_next_vertex();
00272 sendGoal(next_vertex);
00273 }
00274
00275 }
00276
00277 int main(int argc, char** argv) {
00278
00279 DTAGreedy_Agent agent;
00280 agent.init(argc,argv);
00281 agent.run();
00282
00283 return 0;
00284 }