Skip to content

Commit f54d672

Browse files
author
Saurav Agarwal
committed
Add persistent noise
2 parents 46a5d7c + f2f23f3 commit f54d672

File tree

14 files changed

+148
-160
lines changed

14 files changed

+148
-160
lines changed

‎cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -64,14 +64,15 @@ class CentralizedCVT : public AbstractController {
6464
std::vector<double> voronoi_mass_;
6565

6666
bool is_converged_ = false;
67+
bool force_no_noise_ = false;
6768

6869
public:
69-
CentralizedCVT(Parameters const &params, CoverageSystem &env)
70-
: CentralizedCVT(params, params.pNumRobots, env) {}
70+
CentralizedCVT(Parameters const &params, CoverageSystem &env, bool force_no_noise = false)
71+
: CentralizedCVT(params, params.pNumRobots, env, force_no_noise) {}
7172
CentralizedCVT(Parameters const &params, size_t const &num_robots,
72-
CoverageSystem &env)
73-
: params_{params}, num_robots_{num_robots}, env_{env} {
74-
robot_global_positions_ = env_.GetRobotPositions();
73+
CoverageSystem &env, bool force_no_noise = false)
74+
: params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} {
75+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
7576
actions_.resize(num_robots_);
7677
goals_ = robot_global_positions_;
7778
voronoi_mass_.resize(num_robots_, 0);
@@ -102,7 +103,7 @@ class CentralizedCVT : public AbstractController {
102103

103104
int ComputeActions() {
104105
is_converged_ = true;
105-
robot_global_positions_ = env_.GetRobotPositions();
106+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
106107
ComputeGoals();
107108
auto voronoi_cells = voronoi_.GetVoronoiCells();
108109
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {

‎cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,15 @@ class ClairvoyantCVT : public AbstractController {
6363
std::vector<double> voronoi_mass_;
6464

6565
bool is_converged_ = false;
66+
bool force_no_noise_ = false;
6667

6768
public:
68-
ClairvoyantCVT(Parameters const &params, CoverageSystem &env)
69-
: ClairvoyantCVT(params, params.pNumRobots, env) {}
69+
ClairvoyantCVT(Parameters const &params, CoverageSystem &env, bool force_no_noise = false)
70+
: ClairvoyantCVT(params, params.pNumRobots, env, force_no_noise) {}
7071
ClairvoyantCVT(Parameters const &params, size_t const &num_robots,
71-
CoverageSystem &env)
72-
: params_{params}, num_robots_{num_robots}, env_{env} {
73-
robot_global_positions_ = env_.GetRobotPositions();
72+
CoverageSystem &env, bool force_no_noise = false)
73+
: params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} {
74+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
7475
actions_.resize(num_robots_);
7576
goals_ = robot_global_positions_;
7677
voronoi_mass_.resize(num_robots_, 0);
@@ -99,7 +100,7 @@ class ClairvoyantCVT : public AbstractController {
99100

100101
int ComputeActions() {
101102
is_converged_ = true;
102-
robot_global_positions_ = env_.GetRobotPositions();
103+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
103104
ComputeGoals();
104105
auto voronoi_cells = voronoi_.GetVoronoiCells();
105106
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {

‎cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h‎

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -64,14 +64,15 @@ class DecentralizedCVT : public AbstractController {
6464
std::vector<double> voronoi_mass_;
6565

6666
bool is_converged_ = false;
67+
bool force_no_noise_ = false;
6768

6869
public:
69-
DecentralizedCVT(Parameters const &params, CoverageSystem &env)
70-
: DecentralizedCVT(params, params.pNumRobots, env) {}
70+
DecentralizedCVT(Parameters const &params, CoverageSystem &env, bool force_no_noise = false)
71+
: DecentralizedCVT(params, params.pNumRobots, env, force_no_noise) {}
7172
DecentralizedCVT(Parameters const &params, size_t const &num_robots,
72-
CoverageSystem &env)
73-
: params_{params}, num_robots_{num_robots}, env_{env} {
74-
robot_global_positions_ = env_.GetRobotPositions();
73+
CoverageSystem &env, bool force_no_noise = false)
74+
: params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} {
75+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
7576
actions_.resize(num_robots_);
7677
goals_ = robot_global_positions_;
7778
voronoi_mass_.resize(num_robots_, 0);
@@ -80,17 +81,17 @@ class DecentralizedCVT : public AbstractController {
8081

8182
PointVector GetActions() { return actions_; }
8283

83-
PointVector GetGoals() { return goals_; }
84+
auto GetGoals() { return goals_; }
8485

8586
void ComputeGoals() {
8687
#pragma omp parallel for num_threads(num_robots_)
8788
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
88-
Point2 const &pos = robot_global_positions_[iRobot];
89+
auto const &pos = robot_global_positions_[iRobot];
8990
MapUtils::MapBounds index, offset;
9091
MapUtils::ComputeOffsets(params_.pResolution, pos, params_.pLocalMapSize,
9192
params_.pWorldMapSize, index, offset);
92-
MapType robot_map = env_.GetRobotMap(iRobot);
93-
MapType robot_local_map = robot_map.block(index.left + offset.left,
93+
auto robot_map = env_.GetRobotMap(iRobot);
94+
auto robot_local_map = robot_map.block(index.left + offset.left,
9495
index.bottom + offset.bottom,
9596
offset.width, offset.height);
9697
Point2 map_translation(
@@ -114,7 +115,7 @@ class DecentralizedCVT : public AbstractController {
114115
/* std::cout << "Voronoi: " << robot_positions[0][0] << " " <<
115116
* robot_positions[0][1] << std::endl; */
116117
int count = 1;
117-
for (Point2 const &pos : robot_neighbors_pos) {
118+
for (auto const &pos : robot_neighbors_pos) {
118119
robot_positions[count] = pos - map_translation;
119120
++count;
120121
}
@@ -144,7 +145,7 @@ class DecentralizedCVT : public AbstractController {
144145

145146
int ComputeActions() {
146147
is_converged_ = true;
147-
robot_global_positions_ = env_.GetRobotPositions();
148+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
148149
ComputeGoals();
149150
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
150151
actions_[iRobot] = Point2(0, 0);

‎cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -67,14 +67,15 @@ class NearOptimalCVT : public AbstractController {
6767
PointVector goals_, actions_;
6868

6969
bool is_converged_ = false;
70+
bool force_no_noise_ = false;
7071

7172
public:
72-
NearOptimalCVT(Parameters const &params, CoverageSystem &env)
73-
: NearOptimalCVT(params, params.pNumRobots, env) {}
73+
NearOptimalCVT(Parameters const &params, CoverageSystem &env, bool force_no_noise = false)
74+
: NearOptimalCVT(params, params.pNumRobots, env, force_no_noise) {}
7475
NearOptimalCVT(Parameters const &params, size_t const &num_robots,
75-
CoverageSystem &env)
76-
: params_{params}, num_robots_{num_robots}, env_{env} {
77-
robot_global_positions_ = env_.GetRobotPositions();
76+
CoverageSystem &env, bool force_no_noise = false)
77+
: params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} {
78+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
7879
actions_.resize(num_robots_);
7980
goals_ = robot_global_positions_;
8081
ComputeGoals();
@@ -95,7 +96,7 @@ class NearOptimalCVT : public AbstractController {
9596

9697
int ComputeActions() {
9798
is_converged_ = true;
98-
robot_global_positions_ = env_.GetRobotPositions();
99+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
99100
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
100101
actions_[iRobot] = Point2(0, 0);
101102
Point2 diff = goals_[iRobot] - robot_global_positions_[iRobot];

‎cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -77,18 +77,20 @@ class OracleBangExploreExploit : public AbstractController {
7777
double time_step_dist_ = 0;
7878
double eps = 0.0001;
7979
double better_threshold_ = 2;
80+
bool force_no_noise_ = false;
8081

8182
public:
8283
OracleBangExploreExploit(Parameters const &params, size_t const &num_robots,
83-
CoverageSystem &env)
84+
CoverageSystem &env, bool force_no_noise = false)
8485
: params_{params},
8586
num_robots_{num_robots},
8687
env_{env},
88+
force_no_noise_{force_no_noise},
8789
exploration_map_{env.GetSystemExplorationMap()},
8890
explored_idf_map_{env.GetSystemExploredIDFMap()} {
8991
voronoi_cells_.resize(num_robots_);
9092

91-
robot_global_positions_ = env_.GetRobotPositions();
93+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
9294
actions_.resize(num_robots_);
9395
goals_ = robot_global_positions_;
9496
sensor_area_ = params_.pSensorSize * params_.pSensorSize;
@@ -119,7 +121,7 @@ class OracleBangExploreExploit : public AbstractController {
119121
std::vector<VoronoiCell> GetVoronoiCells() { return voronoi_cells_; }
120122

121123
void ManageRobotStatus() {
122-
robot_global_positions_ = env_.GetRobotPositions();
124+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
123125
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
124126
robot_status_[iRobot] = 0;
125127

@@ -341,7 +343,7 @@ class OracleBangExploreExploit : public AbstractController {
341343
int ComputeActions() {
342344
if (first_step_ == true) {
343345
first_step_ = false;
344-
robot_global_positions_ = env_.GetRobotPositions();
346+
robot_global_positions_ = env_.GetRobotPositions(force_no_noise_);
345347
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
346348
goals_[iRobot] = robot_global_positions_[iRobot] +
347349
Point2{rand_dist_(gen_), rand_dist_(gen_)};

‎cppsrc/core/include/CoverageControl/coverage_system.h‎

Lines changed: 13 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -64,9 +64,9 @@ namespace CoverageControl {
6464
* library.
6565
*/
6666
class CoverageSystem {
67-
Parameters const params_; //!< Parameters for the coverage system
68-
std::shared_ptr <WorldIDF> world_idf_ptr_; //!< World IDF object
69-
size_t num_robots_ = 0; //!< Number of robots
67+
Parameters const params_; //!< Parameters for the coverage system
68+
std::shared_ptr<WorldIDF> world_idf_ptr_; //!< World IDF object
69+
size_t num_robots_ = 0; //!< Number of robots
7070
std::vector<RobotModel> robots_; //!< Vector of robots of type RobotModel
7171
double normalization_factor_ = 0; //!< Normalization factor for the world IDF
7272
Voronoi voronoi_; //!< Voronoi object
@@ -78,6 +78,7 @@ class CoverageSystem {
7878
std::uniform_real_distribution<>
7979
distrib_pts_; //!< Uniform distribution for generating random points
8080
PointVector robot_global_positions_; //!< Global positions of the robots
81+
PointVector noisy_robot_global_positions_; //!< Global noisy positions of the robots
8182
MapType
8283
system_map_; //!< System map contains explored and unexplored locations
8384
MapType
@@ -146,6 +147,10 @@ class CoverageSystem {
146147
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
147148
robot_global_positions_[iRobot] =
148149
robots_[iRobot].GetGlobalCurrentPosition();
150+
if (params_.pAddNoisePositions) {
151+
noisy_robot_global_positions_[iRobot] =
152+
robots_[iRobot].GetNoisyGlobalCurrentPosition();
153+
}
149154
}
150155
}
151156

@@ -347,7 +352,8 @@ class CoverageSystem {
347352
}
348353
auto const &history = robot_positions_history_[robot_id];
349354
Point2 const last_pos = history.back();
350-
auto it_end = std::next(history.crbegin(), std::min(6, static_cast<int>(history.size()) - 1));
355+
auto it_end = std::next(history.crbegin(),
356+
std::min(6, static_cast<int>(history.size()) - 1));
351357
bool flag = false;
352358
int count = 0;
353359
std::for_each(history.crbegin(), it_end,
@@ -515,24 +521,17 @@ class CoverageSystem {
515521
PointVector GetRobotPositions(bool force_no_noise = false) {
516522
UpdateRobotPositions();
517523
if (params_.pAddNoisePositions and not force_no_noise) {
518-
PointVector noisy_robot_global_positions;
519-
for (Point2 pt : robot_global_positions_) {
520-
noisy_robot_global_positions.push_back(AddNoise(pt));
521-
}
522-
return noisy_robot_global_positions;
524+
return noisy_robot_global_positions_;
523525
}
524526
return robot_global_positions_;
525527
}
526528

527529
Point2 GetRobotPosition(int const robot_id,
528530
bool force_no_noise = false) const {
529-
Point2 robot_pos;
530-
robot_pos[0] = robots_[robot_id].GetGlobalCurrentPosition()[0];
531-
robot_pos[1] = robots_[robot_id].GetGlobalCurrentPosition()[1];
532531
if (params_.pAddNoisePositions and not force_no_noise) {
533-
return AddNoise(robot_pos);
532+
return robots_[robot_id].GetNoisyGlobalCurrentPosition();
534533
}
535-
return robot_pos;
534+
return robots_[robot_id].GetGlobalCurrentPosition();
536535
}
537536

538537
const MapType &GetRobotLocalMap(size_t const id) {
@@ -569,25 +568,6 @@ class CoverageSystem {
569568
return robots_[id].GetSensorView();
570569
}
571570

572-
auto GetRobotsInCommunication(size_t const id) const {
573-
CheckRobotID(id);
574-
PointVector robot_neighbors_pos;
575-
for (size_t i = 0; i < num_robots_; ++i) {
576-
if (id == i) {
577-
continue;
578-
}
579-
Point2 relative_pos =
580-
robot_global_positions_[i] - robot_global_positions_[id];
581-
if (relative_pos.norm() < params_.pCommunicationRange) {
582-
robot_neighbors_pos.push_back(relative_pos);
583-
}
584-
}
585-
std::sort(
586-
robot_neighbors_pos.begin(), robot_neighbors_pos.end(),
587-
[](Point2 const &a, Point2 const &b) { return a.norm() < b.norm(); });
588-
return robot_neighbors_pos;
589-
}
590-
591571
std::pair<MapType, MapType> GetRobotCommunicationMaps(size_t const, size_t);
592572

593573
std::vector<MapType> GetCommunicationMaps(size_t map_size) {

‎cppsrc/core/include/CoverageControl/parameters.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@
3535
#include <cmath>
3636
#include <string>
3737

38+
#include <array>
39+
3840
namespace CoverageControl {
3941

4042
/*!
@@ -143,7 +145,8 @@ class Parameters {
143145
* @{
144146
*/
145147
bool pAddNoisePositions = false;
146-
double pPositionsNoiseSigma = 0.;
148+
double pPositionsNoiseSigmaMin = 0;
149+
double pPositionsNoiseSigmaMax = 0;
147150
/*! @} */
148151
/*! @} */
149152

0 commit comments

Comments
 (0)