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: Luca Iocchi (2014-2016) 00036 *********************************************************************/ 00037 00038 #include <sstream> 00039 #include <string> 00040 #include <ros/ros.h> 00041 #include <move_base_msgs/MoveBaseAction.h> 00042 #include <actionlib/client/simple_action_client.h> 00043 #include <tf/transform_broadcaster.h> 00044 #include <tf/transform_listener.h> 00045 #include <nav_msgs/Odometry.h> 00046 #include <std_msgs/Int16MultiArray.h> 00047 #include <algorithm> 00048 #include <stdio.h> 00049 00050 #include "PatrolAgent.h" 00051 #include "algorithms.h" 00052 #include "config.h" 00053 00054 00055 #define CONFIG_FILENAME "params/DTA/DTASSI.params" 00056 00057 #define BIG_NUMBER 999999 00058 00059 #define DEBUG_PRINT 0 00060 00061 //a tuple <robotId,bidValue> 00062 typedef struct bid_tuple { 00063 double bidValue; 00064 int robotId; 00065 } bid_tuple; 00066 00067 00068 class SSIPatrolAgent: public PatrolAgent { 00069 00070 protected: 00071 // Mutex to update global_idleness safely 00072 pthread_mutex_t lock; 00073 00074 //true if I am selecting the first vertex to go to 00075 bool first_vertex; 00076 00077 //the goal I will go when I reach the current goal 00078 //required to avoid that the robot waits idle after completing each goal 00079 int next_next_vertex; 00080 00081 00082 double *global_instantaneous_idleness; // global estimated idleness 00083 double last_update_idl; 00084 00085 //time of task requests arrived for each room. A vector of integer, where taskRequest[i] 00086 //is the time at which this robot received a task request for location i 00087 int* taskRequests; 00088 00089 //tasks this robot is responsible for. A boolean vector, where the value of index i is 1 if I have to visit that location 00090 bool* tasks; 00091 00092 // number of active tasks 00093 int nactivetasks; 00094 00095 //vertices that has been selected within the same optimization loop but for which the robot did not win the auction 00096 bool* selected_vertices; 00097 00098 //waiting time for collecting all bids from other robots 00099 double timeout; 00100 00101 //threshold on idleness for considering a location just visited 00102 double threshold; 00103 00104 //bids received from other robots. A vector indexed by locations. 00105 //bids[i] = <robotId, bidValue>, where bidValue is the minimum bid received for location i and senderId is the sender 00106 bid_tuple* bids; 00107 00108 //parameter to weight Idleness in the util computation 00109 double theta_idl; 00110 00111 //parameter to weight navigation cost in the util computation 00112 double theta_cost; 00113 00114 //parameter to weight hop cost in the util computation 00115 double theta_hop; 00116 00117 //increase the value of a bid when received to avoid switching when there is no real gain, put this to zero to avoid using histeresys 00118 //0 -> less interferences, high stddev 00119 double hist; 00120 00121 ConfigFile cf; 00122 00123 //allocate an array of bool one for each vertex, set all to false 00124 bool* create_selected_vertices(); 00125 00126 //set all selected vertices to false, set current_vertex to true (avoid considering current_vertex as next target) 00127 void reset_selected_vertices(bool* sv); 00128 00129 //set all selected vertices to true, set neighbouring vertices to false (consider only neighbouring vertices as next target) 00130 void select_faraway_vertices(bool* selected_vertices, int cv); 00131 00132 //check if all vertices have been selected 00133 bool all_selected(bool* sv); 00134 00135 //select the next best vertex (used to select which task should be auctioned); 00136 //mark vertex that have been selected to avoid selecting them in the same alg step 00137 //when all vertices have been selected reset the selection excluding the current_vertex 00138 int select_next_vertex(int currv, bool* sv); 00139 00140 //select the next best vertex (used to select which task should be auctioned); 00141 //mark vertex that have been selected to avoid selecting them in the same alg step 00142 int return_next_vertex(int currv, bool* sv); 00143 00144 00145 //compute minimum path cost considering all tasks (tasks) and the next vertex (nv). 00146 //The first room is always the current goal (if any), then rooms are visited in decreasing order of utility. 00147 //The path cost is sum of travel cost given the order. 00148 virtual double compute_bid(int nv); 00149 00150 //force the best bid for dest to be the one from robotId with value bidvalue 00151 void force_bid(int nv,double bidvalue,int robotId); 00152 00153 //update tasks seeting to true only the vertices for which this robot has the current highest bid 00154 //based on the array bids 00155 virtual void update_tasks(); 00156 00157 //announce the intension to go to vertex nv with a bid value of bv 00158 void send_target(int nv,double bv); 00159 00160 //computes whether this robot holds the best bid for vertex nv 00161 //based on the array bids 00162 bool best_bid(int nv); 00163 00164 //computes whether this robot holds the best bid for vertex nv or whether a greedy vertex assignment should be performed 00165 //a greedy action is performed when the robot (cv) is adjacent to the next vertex (nv), the current idleness of nv is higher than 2*std_dev + mean 00166 // (std_dev and mean refer to the vector of current idleness) and no robot is going towards nv (i.e., no one sent a 0 valued bid). 00167 bool greedy_best_bid(int cv, int nv); 00168 00169 //return geometric distance from current robot position to vertex 00170 double compute_distance(int vertex); 00171 00172 //return path cost from vertex cv to vertex nv 00173 double compute_cost(int cv, int nv); 00174 00175 //compute number of hops to nv from cv 00176 size_t compute_hops(int cv, int nv); 00177 00178 virtual void update_bids(int next_vertex, double bid_value, int senderId); 00179 00180 void send_bid(int nv,double bv); 00181 00182 void idleness_msg_handler(std::vector<int>::const_iterator it); 00183 00184 void task_request_msg_handler(std::vector<int>::const_iterator it, int sender_id); 00185 00186 void bid_msg_handler(std::vector<int>::const_iterator it, int sender_id); 00187 00188 void wait(); 00189 00190 00191 public: 00192 00193 SSIPatrolAgent(); 00194 00195 virtual void init(int argc, char** argv); 00196 virtual void onGoalComplete(); 00197 virtual int compute_next_vertex(); 00198 virtual void send_results(); 00199 virtual void receive_results(); 00200 00201 int compute_next_vertex(int cv); 00202 double compute_cost(int vertex); 00203 virtual double utility(int currentv, int nextv); 00204 void update_global_idleness(); 00205 }; 00206