SSIPatrolAgent.h
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: 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 


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