Skip to content

Commit 8af7b23

Browse files
author
Saurav Agarwal
committed
Add mutables for cl
1 parent d13018a commit 8af7b23

4 files changed

Lines changed: 43 additions & 15 deletions

File tree

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -80,17 +80,17 @@ class DecentralizedCVT : public AbstractController {
8080

8181
PointVector GetActions() { return actions_; }
8282

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

8585
void ComputeGoals() {
8686
#pragma omp parallel for num_threads(num_robots_)
8787
for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
88-
auto const &pos = robot_global_positions_[iRobot];
88+
Point2 const &pos = robot_global_positions_[iRobot];
8989
MapUtils::MapBounds index, offset;
9090
MapUtils::ComputeOffsets(params_.pResolution, pos, params_.pLocalMapSize,
9191
params_.pWorldMapSize, index, offset);
92-
auto robot_map = env_.GetRobotMap(iRobot);
93-
auto robot_local_map = robot_map.block(index.left + offset.left,
92+
MapType robot_map = env_.GetRobotMap(iRobot);
93+
MapType robot_local_map = robot_map.block(index.left + offset.left,
9494
index.bottom + offset.bottom,
9595
offset.width, offset.height);
9696
Point2 map_translation(
@@ -114,7 +114,7 @@ class DecentralizedCVT : public AbstractController {
114114
/* std::cout << "Voronoi: " << robot_positions[0][0] << " " <<
115115
* robot_positions[0][1] << std::endl; */
116116
int count = 1;
117-
for (auto const &pos : robot_neighbors_pos) {
117+
for (Point2 const &pos : robot_neighbors_pos) {
118118
robot_positions[count] = pos - map_translation;
119119
++count;
120120
}

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

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -231,6 +231,18 @@ class CoverageSystem {
231231
robots_[robot_id].SetGlobalRobotPosition(global_pos);
232232
PostStepCommands(robot_id);
233233
}
234+
//
235+
//! Set the global positions of all robots
236+
void SetGlobalRobotPositions(PointVector const &global_positions) {
237+
if (global_positions.size() != num_robots_) {
238+
throw std::length_error{
239+
"The size of the positions don't match with the number of robots"};
240+
}
241+
for (size_t i = 0; i < num_robots_; ++i) {
242+
robots_[i].SetGlobalRobotPosition(global_positions[i]);
243+
}
244+
PostStepCommands();
245+
}
234246

235247
//! Set the positions of all robots with respect to their current positions
236248
//! \note Same as SetLocalRobotPositions
@@ -457,11 +469,17 @@ class CoverageSystem {
457469
const MapType &GetSystemMap() const { return system_map_; }
458470
const MapType &GetSystemExplorationMap() const { return exploration_map_; }
459471
const MapType &GetSystemExploredIDFMap() const { return explored_idf_map_; }
472+
MapType &GetSystemExploredIDFMapMutable() { return explored_idf_map_; }
460473
//! Get the world map
461474
const MapType &GetWorldMap() const { return world_idf_.GetWorldMap(); }
462475
//! Get the world map (mutable)
463476
MapType &GetWorldMapMutable() { return world_idf_.GetWorldMapMutable(); }
464477

478+
MapType &GetRobotMapMutable(size_t const id) {
479+
CheckRobotID(id);
480+
return robots_[id].GetRobotMapMutable();
481+
}
482+
465483
inline auto GetNumRobots() const { return num_robots_; }
466484
inline auto GetNumFeatures() const { return num_robots_; }
467485

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

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -173,9 +173,7 @@ class RobotModel {
173173
if (params_.pUpdateSensorView == true) {
174174
UpdateSensorView();
175175
}
176-
if (params_.pUpdateRobotMap == false) {
177-
robot_map_ = world_idf_->GetWorldMap();
178-
} else {
176+
if (params_.pUpdateRobotMap == true) {
179177
UpdateRobotMap();
180178
}
181179
}
@@ -283,9 +281,10 @@ class RobotModel {
283281
return system_map_;
284282
}
285283

286-
const MapType &GetRobotLocalMap() {
287-
/* local_map_ = MapType::Constant(params_.pLocalMapSize,
288-
* params_.pLocalMapSize, -1.0); */
284+
// const MapType &GetRobotLocalMap() {
285+
/* local_map_ = MapType::Constant(params_.pLocalMapSize,
286+
* params_.pLocalMapSize, -1.0); */
287+
void ComputeLocalMap() {
289288
local_map_ =
290289
MapType::Constant(params_.pLocalMapSize, params_.pLocalMapSize, 0.);
291290
if (not MapUtils::IsPointOutsideBoundary(
@@ -295,6 +294,11 @@ class RobotModel {
295294
params_.pRobotMapSize, robot_map_,
296295
params_.pLocalMapSize, local_map_);
297296
}
297+
}
298+
MapType &GetRobotMapMutable() { return robot_map_; }
299+
300+
const MapType &GetRobotLocalMap() {
301+
ComputeLocalMap();
298302
return local_map_;
299303
}
300304

‎cppsrc/python_bindings/core_binds.h‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,6 @@
4949
#include <CoverageControl/algorithms/oracle_explore_exploit.h>
5050
#include <CoverageControl/algorithms/simul_explore_exploit.h>
5151

52-
#include <iostream>
53-
#include <string>
5452
#include <vector>
5553

5654
namespace py = pybind11;
@@ -147,8 +145,6 @@ void pyCoverageControl_core(py::module &m) {
147145
.def("SetGlobalRobotPosition", &RobotModel::SetGlobalRobotPosition)
148146
.def("GetGlobalStartPosition", &RobotModel::GetGlobalStartPosition)
149147
.def("GetGlobalCurrentPosition", &RobotModel::GetGlobalCurrentPosition)
150-
.def("GetRobotMap", &RobotModel::GetRobotMap,
151-
py::return_value_policy::reference_internal)
152148
.def("GetRobotLocalMap", &RobotModel::GetRobotLocalMap,
153149
py::return_value_policy::reference_internal)
154150
.def("GetRobotSystemMap", &RobotModel::GetRobotSystemMap,
@@ -294,6 +290,7 @@ void pyCoverageControl_core_coverage_system(py::module &m) {
294290
.def("SetLocalRobotPositions", &CoverageSystem::SetLocalRobotPositions)
295291
.def("SetLocalRobotPosition", &CoverageSystem::SetLocalRobotPosition)
296292
.def("SetGlobalRobotPosition", &CoverageSystem::SetGlobalRobotPosition)
293+
.def("SetGlobalRobotPositions", &CoverageSystem::SetGlobalRobotPositions)
297294
.def("GetRelativePositonsNeighbors",
298295
&CoverageSystem::GetRelativePositonsNeighbors)
299296
.def("SetRobotPositions", &CoverageSystem::SetRobotPositions)
@@ -306,11 +303,20 @@ void pyCoverageControl_core_coverage_system(py::module &m) {
306303
"Get Positions of Robots", py::arg("force_no_noise") = false)
307304
.def("GetRobotLocalMap", &CoverageSystem::GetRobotLocalMap,
308305
py::return_value_policy::reference_internal)
306+
.def("GetRobotMap", &CoverageSystem::GetRobotMap,
307+
py::return_value_policy::reference_internal)
308+
.def("GetRobotMapMutable", &CoverageSystem::GetRobotMapMutable,
309+
py::return_value_policy::reference_internal)
309310
.def("GetRobotSensorView", &CoverageSystem::GetRobotSensorView,
310311
py::return_value_policy::reference_internal)
311312
.def("GetCommunicationMaps", &CoverageSystem::GetCommunicationMaps)
312313
.def("GetRobotsInCommunication",
313314
&CoverageSystem::GetRobotsInCommunication)
315+
.def("GetSystemExploredIDFMap", &CoverageSystem::GetSystemExploredIDFMap,
316+
py::return_value_policy::reference_internal)
317+
.def("GetSystemExploredIDFMapMutable",
318+
&CoverageSystem::GetSystemExploredIDFMapMutable,
319+
py::return_value_policy::reference_internal)
314320
.def("ComputeVoronoiCells", &CoverageSystem::ComputeVoronoiCells,
315321
py::return_value_policy::reference_internal)
316322
.def("GetVoronoiCells", &CoverageSystem::GetVoronoiCells,

0 commit comments

Comments
 (0)