-
Notifications
You must be signed in to change notification settings - Fork 6
Expand file tree
/
Copy pathcoverage_algorithm.cpp
More file actions
95 lines (85 loc) · 3.27 KB
/
coverage_algorithm.cpp
File metadata and controls
95 lines (85 loc) · 3.27 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
/*!
* \file coverage_algorithm.cpp
* \brief Program to test the CVT-based (Lloyd's) coverage control algorithms
*
* This program is used to execute CVT-based (Lloyd's) coverage control
* algorithms. The environment can be initialized with default parameters. The
* program can also take in a parameter file, a position file and an IDF file.
* The position file contains the initial positions of the robots and the IDF
* file contains the location of the features of interest. The program then runs
* the coverage control algorithm and outputs the final objective value.
* ```bash
* ./coverage_algorithm [parameter_file] [<position_file> <idf_file>]
* ```
*
*/
#include <CoverageControl/algorithms/clairvoyant_cvt.h>
#include <CoverageControl/coverage_system.h>
#include <CoverageControl/parameters.h>
#include <CoverageControl/typedefs.h>
#include <CoverageControl/world_idf.h>
// #include <CoverageControl/algorithms/centralized_cvt.h>
// #include <CoverageControl/algorithms/decentralized_cvt.h>
// #include <CoverageControl/algorithms/near_optimal_cvt.h>
// #include <CoverageControl/algorithms/simul_explore_exploit.h>
/* #include <CoverageControl/bivariate_normal_distribution.h> */
#include <iostream>
#include <memory>
#include <string>
typedef CoverageControl::ClairvoyantCVT CoverageAlgorithm;
/* typedef CoverageControl::CentralizedCVT CoverageAlgorithm; */
/* typedef CoverageControl::DecentralizedCVT CoverageAlgorithm; */
/* typedef CoverageControl::DecentralizedCVT CoverageAlgorithm; */
/* typedef CoverageControl::NearOptimalCVT CoverageAlgorithm; */
using CoverageControl::CoverageSystem;
using CoverageControl::Parameters;
using CoverageControl::Point2;
using CoverageControl::PointVector;
using CoverageControl::WorldIDF;
int main(int argc, char** argv) {
CoverageControl::CudaUtils::SetUseCuda(false);
Parameters params;
/* params.pSensorSize = 16; */
if (argc >= 2) {
std::string parameter_file = argv[1];
params = Parameters(parameter_file);
}
std::unique_ptr<CoverageSystem> env;
if (argc == 3) {
std::cerr << "Please provide both position and IDF files" << std::endl;
std::cerr << "Usage: ./coverage_algorithm [parameter_file] "
"[<position_file> <idf_file>]"
<< std::endl;
return 1;
} else if (argc == 4) {
std::string pos_file = argv[2];
std::string idf_file = argv[3];
WorldIDF world_idf(params, idf_file);
env = std::make_unique<CoverageSystem>(params, world_idf, pos_file);
} else {
env = std::make_unique<CoverageSystem>(params);
}
auto init_objective = env->GetObjectiveValue();
std::cout << "Initial objective: " << init_objective << std::endl;
CoverageAlgorithm algorithm(params, *env);
auto goals = algorithm.GetGoals();
for (int ii = 0; ii < params.pEpisodeSteps; ++ii) {
algorithm.ComputeActions();
auto actions = algorithm.GetActions();
if (env->StepActions(actions)) {
std::cout << "Invalid action" << std::endl;
break;
}
if (ii % 100 == 0) {
std::cout << "Step: " << ii << std::endl;
}
if (algorithm.IsConverged()) {
break;
}
}
auto final_objective = env->GetObjectiveValue();
std::cout << "Improvement %: "
<< (init_objective - final_objective) / init_objective * 100
<< std::endl;
return 0;
}