|
5 | 5 | |
6 | 6 | * Repository: https://github.com/KumarRobotics/CoverageControl |
7 | 7 | * |
8 | | - * The CoverageControl library is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. |
| 8 | + * Copyright (c) 2024, Saurav Agarwal |
9 | 9 | * |
10 | | - * The CoverageControl library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. |
| 10 | + * The CoverageControl library is free software: you can redistribute it and/or |
| 11 | + * modify it under the terms of the GNU General Public License as published by |
| 12 | + * the Free Software Foundation, either version 3 of the License, or (at your |
| 13 | + * option) any later version. |
11 | 14 | * |
12 | | - * You should have received a copy of the GNU General Public License along with CoverageControl library. If not, see <https://www.gnu.org/licenses/>. |
| 15 | + * The CoverageControl library is distributed in the hope that it will be |
| 16 | + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 17 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
| 18 | + * Public License for more details. |
| 19 | + * |
| 20 | + * You should have received a copy of the GNU General Public License along with |
| 21 | + * CoverageControl library. If not, see <https://www.gnu.org/licenses/>. |
13 | 22 | */ |
14 | 23 |
|
15 | 24 | /*! |
16 | 25 | * \file centralized_cvt.h |
17 | 26 | * \brief Contains the class CentralizedCVT |
18 | 27 | */ |
19 | 28 |
|
20 | | -#ifndef COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ |
21 | | -#define COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ |
| 29 | +#ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ |
| 30 | +#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ |
22 | 31 |
|
23 | | -#include <vector> |
| 32 | +#include <omp.h> |
| 33 | + |
| 34 | +#include <algorithm> |
24 | 35 | #include <fstream> |
25 | 36 | #include <iostream> |
| 37 | +#include <queue> |
26 | 38 | #include <random> |
27 | | -#include <algorithm> |
28 | 39 | #include <set> |
29 | | -#include <queue> |
30 | | -#include <omp.h> |
| 40 | +#include <vector> |
31 | 41 |
|
32 | | -#include "../parameters.h" |
33 | | -#include "../typedefs.h" |
34 | | -#include "../coverage_system.h" |
| 42 | +#include "CoverageControl/algorithms/abstract_controller.h" |
| 43 | +#include "CoverageControl/coverage_system.h" |
| 44 | +#include "CoverageControl/parameters.h" |
| 45 | +#include "CoverageControl/typedefs.h" |
35 | 46 |
|
36 | 47 | namespace CoverageControl { |
37 | 48 |
|
38 | | - /*! |
39 | | - * The coverage control algorithm uses centralized Centroidal Voronoi Tessellation (CVT) or Lloyd's algorithm on accumulated local map of individual robots, i.e., a robot has knowledge only about the regions it has visited. |
40 | | - * Communication radius is not considered. |
41 | | - * The algorithm is online---it takes localized actions based on the current robot positions. |
42 | | - **/ |
43 | | - class CentralizedCVT { |
44 | | - private: |
45 | | - Parameters const params_; |
46 | | - size_t num_robots_ = 0; |
47 | | - CoverageSystem &env_; |
48 | | - Voronoi voronoi_; |
49 | | - PointVector robot_global_positions_; |
50 | | - PointVector goals_, actions_; |
51 | | - std::vector <double> voronoi_mass_; |
52 | | - |
53 | | - bool continue_flag_ = false; |
54 | | - |
55 | | - public: |
56 | | - CentralizedCVT ( |
57 | | - Parameters const ¶ms, |
58 | | - size_t const &num_robots, |
59 | | - CoverageSystem &env) : |
60 | | - params_{params}, |
61 | | - num_robots_{num_robots}, |
62 | | - env_{env} { |
63 | | - |
64 | | - robot_global_positions_ = env_.GetRobotPositions(); |
65 | | - actions_.resize(num_robots_); |
66 | | - goals_ = robot_global_positions_; |
67 | | - voronoi_mass_.resize(num_robots_, 0); |
68 | | - voronoi_ = Voronoi(robot_global_positions_, env_.GetSystemExploredIDFMap(), Point2(params_.pWorldMapSize, params_.pWorldMapSize), params_.pResolution); |
69 | | - ComputeGoals(); |
70 | | - } |
71 | | - |
72 | | - PointVector GetActions() { return actions_; } |
73 | | - |
74 | | - auto GetGoals() { return goals_; } |
75 | | - |
76 | | - auto &GetVoronoi() { return voronoi_; } |
77 | | - |
78 | | - void ComputeGoals() { |
79 | | - voronoi_ = Voronoi(robot_global_positions_, env_.GetSystemExploredIDFMap(), Point2(params_.pWorldMapSize, params_.pWorldMapSize), params_.pResolution); |
80 | | - auto voronoi_cells = voronoi_.GetVoronoiCells(); |
81 | | - for(size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { |
82 | | - goals_[iRobot] = voronoi_cells[iRobot].centroid(); |
83 | | - voronoi_mass_[iRobot] = voronoi_cells[iRobot].mass(); |
84 | | - } |
85 | | - } |
86 | | - |
87 | | - bool Step() { |
88 | | - continue_flag_ = false; |
89 | | - robot_global_positions_ = env_.GetRobotPositions(); |
90 | | - ComputeGoals(); |
91 | | - auto voronoi_cells = voronoi_.GetVoronoiCells(); |
92 | | - for(size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { |
93 | | - actions_[iRobot] = Point2(0, 0); |
94 | | - Point2 diff = goals_[iRobot] - robot_global_positions_[iRobot]; |
95 | | - double dist = diff.norm(); |
96 | | - /* if(dist < 0.1 * params_.pResolution) { */ |
97 | | - /* continue; */ |
98 | | - /* } */ |
99 | | - if(dist < kEps) { |
100 | | - continue; |
101 | | - } |
102 | | - if(env_.CheckOscillation(iRobot)) { |
103 | | - continue; |
104 | | - } |
105 | | - double speed = dist / params_.pTimeStep; |
106 | | - /* double speed = 2 * dist * voronoi_mass_[iRobot]; */ |
107 | | - speed = std::min(params_.pMaxRobotSpeed, speed); |
108 | | - Point2 direction(diff); |
109 | | - direction.normalize(); |
110 | | - actions_[iRobot] = speed * direction; |
111 | | - continue_flag_ = true; |
112 | | - } |
113 | | - return continue_flag_; |
114 | | - } |
115 | | - |
116 | | - }; |
| 49 | +/*! |
| 50 | + * \addtogroup cpp_api |
| 51 | + * @{ |
| 52 | + * \class CentralizedCVT |
| 53 | + * @} |
| 54 | + * The coverage control algorithm uses centralized Centroidal Voronoi |
| 55 | + *Tessellation (CVT) or Lloyd's algorithm on accumulated local map of individual |
| 56 | + *robots, i.e., a robot has knowledge only about the regions it has visited. |
| 57 | + * Communication radius is not considered. |
| 58 | + * The algorithm is online---it takes localized actions based on the current |
| 59 | + *robot positions. |
| 60 | + **/ |
| 61 | +class CentralizedCVT : public AbstractController { |
| 62 | + private: |
| 63 | + Parameters const params_; |
| 64 | + size_t num_robots_ = 0; |
| 65 | + CoverageSystem &env_; |
| 66 | + Voronoi voronoi_; |
| 67 | + PointVector robot_global_positions_; |
| 68 | + PointVector goals_, actions_; |
| 69 | + std::vector<double> voronoi_mass_; |
| 70 | + |
| 71 | + bool is_converged_ = false; |
| 72 | + |
| 73 | + public: |
| 74 | + CentralizedCVT(Parameters const ¶ms, CoverageSystem &env) |
| 75 | + : CentralizedCVT(params, params.pNumRobots, env) {} |
| 76 | + CentralizedCVT(Parameters const ¶ms, size_t const &num_robots, |
| 77 | + CoverageSystem &env) |
| 78 | + : params_{params}, num_robots_{num_robots}, env_{env} { |
| 79 | + robot_global_positions_ = env_.GetRobotPositions(); |
| 80 | + actions_.resize(num_robots_); |
| 81 | + goals_ = robot_global_positions_; |
| 82 | + voronoi_mass_.resize(num_robots_, 0); |
| 83 | + voronoi_ = Voronoi(robot_global_positions_, env_.GetSystemExploredIDFMap(), |
| 84 | + Point2(params_.pWorldMapSize, params_.pWorldMapSize), |
| 85 | + params_.pResolution); |
| 86 | + ComputeGoals(); |
| 87 | + } |
| 88 | + |
| 89 | + PointVector GetActions() { return actions_; } |
| 90 | + |
| 91 | + auto GetGoals() { return goals_; } |
| 92 | + |
| 93 | + auto &GetVoronoi() { return voronoi_; } |
| 94 | + |
| 95 | + void ComputeGoals() { |
| 96 | + voronoi_ = Voronoi(robot_global_positions_, env_.GetSystemExploredIDFMap(), |
| 97 | + Point2(params_.pWorldMapSize, params_.pWorldMapSize), |
| 98 | + params_.pResolution); |
| 99 | + auto voronoi_cells = voronoi_.GetVoronoiCells(); |
| 100 | + for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { |
| 101 | + goals_[iRobot] = voronoi_cells[iRobot].centroid(); |
| 102 | + voronoi_mass_[iRobot] = voronoi_cells[iRobot].mass(); |
| 103 | + } |
| 104 | + } |
| 105 | + |
| 106 | + bool IsConverged() const { return is_converged_; } |
| 107 | + |
| 108 | + bool ComputeActions() { |
| 109 | + is_converged_ = true; |
| 110 | + robot_global_positions_ = env_.GetRobotPositions(); |
| 111 | + ComputeGoals(); |
| 112 | + auto voronoi_cells = voronoi_.GetVoronoiCells(); |
| 113 | + for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { |
| 114 | + actions_[iRobot] = Point2(0, 0); |
| 115 | + Point2 diff = goals_[iRobot] - robot_global_positions_[iRobot]; |
| 116 | + double dist = diff.norm(); |
| 117 | + /* if (dist < 0.1 * params_.pResolution) { */ |
| 118 | + /* continue; */ |
| 119 | + /* } */ |
| 120 | + if (dist < kEps) { |
| 121 | + continue; |
| 122 | + } |
| 123 | + if (env_.CheckOscillation(iRobot)) { |
| 124 | + continue; |
| 125 | + } |
| 126 | + double speed = dist / params_.pTimeStep; |
| 127 | + /* double speed = 2 * dist * voronoi_mass_[iRobot]; */ |
| 128 | + speed = std::min(params_.pMaxRobotSpeed, speed); |
| 129 | + Point2 direction(diff); |
| 130 | + direction.normalize(); |
| 131 | + actions_[iRobot] = speed * direction; |
| 132 | + is_converged_ = false; |
| 133 | + } |
| 134 | + return true; |
| 135 | + } |
| 136 | + |
| 137 | +}; |
117 | 138 |
|
118 | 139 | } /* namespace CoverageControl */ |
119 | | -#endif /* COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ */ |
| 140 | +#endif // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_CENTRALIZED_CVT_H_ |
0 commit comments