1 #ifndef AREA_DIVISION_LIB_H 2 #define AREA_DIVISION_LIB_H 8 #include <geometry_msgs/PoseStamped.h> 9 #include <nav_msgs/OccupancyGrid.h> 37 nav_msgs::OccupancyGrid get_grid (nav_msgs::OccupancyGrid map,
string cps);
43 void initialize_cps (map<
string, vector<int>> cpss);
51 void initialize_map (
int r,
int c, vector<signed char> src);
58 void assign (vector<valarray<double>> matrix);
66 valarray<float> CalcConnectedMultiplier (valarray<float> dist1, valarray<float> dist2);
75 valarray<double> FinalUpdateOnMetricMatrix (
double CM, valarray<double> curentONe, valarray<float> CC);
82 bool isThisAGoalState (
int thres);
160 #endif // AREA_DIVISION_LIB_H valarray< int > A
Area assignment matrix.
map< string, int > uuid_map
UUID mapping of CPSs.
A class to divide the environment optimally among multiple cyber physical systems (CPSs)...
int max_iter
Maximum number of iterations of the optimization process.
vector< bool > regions
Whether the regions of the CPSs are connected.
vector< vector< int > > cps
Positions of the CPSs.
int discr
Maximum difference between number of assigned grid cells to each CPS.
int rows
Number of rows in the grid map.
double variate_weight
Maximum variate weight of connected components.
vector< valarray< int > > BWlist
A binary array for each CPS which indicates free space.
vector< int > ArrayOfElements
Number of grid cells assigned to each CPS.
bool success
Whether the assignment succeeded.
int ob
Number of obstacles.
int cols
Number of columns in the grid map.
vector< signed char > gridmap
The environment grid map. Robots are represented by 2, obstacles by 1/-2, empty cells by -1...