cpp and py compat

Saurav Agarwal · Saurav Agarwal · commit 287edc021f3a · 2024-03-13T02:09:10.000-04:00
diff --git a/cppsrc/core/include/CoverageControl/algorithms/abstract_controller.h b/cppsrc/core/include/CoverageControl/algorithms/abstract_controller.h
@@ -54,9 +54,9 @@ class AbstractController {
 
   /*!
    * Pure virtual function to compute the actions for the robots
-   * \return True if the actions are computed successfully, false otherwise
+   * \return 0 if the actions are computed successfully, 1 otherwise
    **/
-  virtual bool ComputeActions() = 0;
+  virtual int ComputeActions() = 0;
 };
 
 } /* namespace CoverageControl */
diff --git a/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h
@@ -105,7 +105,7 @@ class CentralizedCVT : public AbstractController {
 
   bool IsConverged() const { return is_converged_; }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     is_converged_ = true;
     robot_global_positions_ = env_.GetRobotPositions();
     ComputeGoals();
@@ -131,7 +131,7 @@ class CentralizedCVT : public AbstractController {
       actions_[iRobot] = speed * direction;
       is_converged_ = false;
     }
-    return true;
+    return 0;
   }
 };
 
diff --git a/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h
@@ -80,7 +80,7 @@ class ClairvoyantCVT : public AbstractController {
     actions_.resize(num_robots_);
     goals_ = robot_global_positions_;
     voronoi_mass_.resize(num_robots_, 0);
-    voronoi_ = Voronoi(robot_global_positions_, env_.GetWorldIDF(),
+    voronoi_ = Voronoi(robot_global_positions_, env_.GetWorldMap(),
                        Point2(params_.pWorldMapSize, params_.pWorldMapSize),
                        params_.pResolution);
     ComputeGoals();
@@ -101,7 +101,7 @@ class ClairvoyantCVT : public AbstractController {
     }
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     is_converged_ = true;
     robot_global_positions_ = env_.GetRobotPositions();
     ComputeGoals();
@@ -127,7 +127,7 @@ class ClairvoyantCVT : public AbstractController {
       actions_[iRobot] = speed * direction;
       is_converged_ = false;
     }
-    return true;
+    return 0;
   }
 
   bool IsConverged() const { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h
@@ -147,7 +147,7 @@ class DecentralizedCVT : public AbstractController {
     }
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     is_converged_ = true;
     robot_global_positions_ = env_.GetRobotPositions();
     ComputeGoals();
@@ -173,8 +173,8 @@ class DecentralizedCVT : public AbstractController {
       direction.normalize();
       actions_[iRobot] = speed * direction;
       is_converged_ = false;
-      return true;
     }
+    return 0;
   }
 
   bool IsConverged() const { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h
@@ -96,11 +96,11 @@ class NearOptimalCVT : public AbstractController {
   void ComputeGoals() {
     goals_ = NearOptimalCVTAlgorithm(
         params_.pLloydNumTries, params_.pLloydMaxIterations, num_robots_,
-        env_.GetWorldIDF(), params_.pWorldMapSize, params_.pResolution,
+        env_.GetWorldMap(), params_.pWorldMapSize, params_.pResolution,
         robot_global_positions_, voronoi_);
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     is_converged_ = true;
     robot_global_positions_ = env_.GetRobotPositions();
     for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
@@ -117,7 +117,7 @@ class NearOptimalCVT : public AbstractController {
       actions_[iRobot] = speed * direction;
       is_converged_ = false;
     }
-    return true;
+    return 0;
   }
 
   bool IsConverged() { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h b/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h
@@ -338,7 +338,7 @@ class OracleBangExploreExploit : public AbstractController {
     /* } */
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     if (first_step_ == true) {
       first_step_ = false;
       robot_global_positions_ = env_.GetRobotPositions();
@@ -368,7 +368,7 @@ class OracleBangExploreExploit : public AbstractController {
         is_converged_ = false;
       }
     }
-    return true;
+    return 0;
   }
 
   bool IsConverged() { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/algorithms/oracle_explore_exploit.h b/cppsrc/core/include/CoverageControl/algorithms/oracle_explore_exploit.h
@@ -159,7 +159,7 @@ class OracleExploreExploit : public AbstractController {
     }
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     if (start_exploit_ == false) {
       if (recompute_goal_counter_ >= recompute_goal_steps_ or
           is_converged_ == true) {
@@ -170,7 +170,7 @@ class OracleExploreExploit : public AbstractController {
     }
     is_converged_ = not env_.StepRobotsToGoals(goals_, actions_);
     UpdateOracleMap();
-    return true;
+    return 0;
   }
 
   bool IsConverged() { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/algorithms/simul_explore_exploit.h b/cppsrc/core/include/CoverageControl/algorithms/simul_explore_exploit.h
@@ -334,7 +334,7 @@ class OracleSimulExploreExploit {
      * std::endl; */
   }
 
-  bool ComputeActions() {
+  int ComputeActions() {
     robot_global_positions_ = env_.GetRobotPositions();
     goals_ = robot_global_positions_;
     if (first_step_ == true) {
@@ -378,7 +378,7 @@ class OracleSimulExploreExploit {
       }
       is_converged_ = false;
     }
-    return true;
+    return 0;
   }
 
   bool IsConverged() { return is_converged_; }
diff --git a/cppsrc/core/include/CoverageControl/bivariate_normal_distribution.h b/cppsrc/core/include/CoverageControl/bivariate_normal_distribution.h
@@ -162,6 +162,15 @@ class BivariateNormalDistribution {
     return scale_ * std::erfc(transformed_point.x() * kOneBySqrt2) *
            std::erfc(transformed_point.y() * kOneBySqrt2) / 4.;
   }
+
+  float IntegrateQuarterPlaneF(Point2 const &point) const {
+    auto transformed_point = TransformPoint(point);
+    float x = static_cast<float>(transformed_point.x());
+    float y = static_cast<float>(transformed_point.y());
+    float scale = static_cast<float>(scale_);
+    return scale * std::erfc(x * kOneBySqrt2f) * std::erfc(y * kOneBySqrt2f) /
+           4.f;
+  }
 };
 
 } /* namespace CoverageControl */
diff --git a/cppsrc/core/include/CoverageControl/constants.h b/cppsrc/core/include/CoverageControl/constants.h
@@ -50,6 +50,7 @@ double const kEps = 1e-10;          /*!< Epsilon for double comparison */
 double const kLargeEps = 1e-4;      /*!< Large epsilon for double comparison */
 double const kSqrt2 = std::sqrt(2); /*!< Square root of 2 */
 double const kOneBySqrt2 = 1. / std::sqrt(2); /*!< 1 by square root of 2 */
+float const kOneBySqrt2f = 1.f / std::sqrt(2.f); /*!< 1 by square root of 2 */
 double const kInfD =
     std::numeric_limits<double>::infinity(); /*!< Infinity for double */
 constexpr auto kMaxPrecision{std::numeric_limits<long double>::digits10 +
diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h
@@ -250,7 +250,6 @@ class CoverageSystem {
   //! Set the world IDF and recompute the world map
   void SetWorldIDF(WorldIDF const &world_idf) {
     world_idf_ = world_idf;
-    world_idf_.GenerateMap();
     normalization_factor_ = world_idf_.GetNormalizationFactor();
   }
   //! @}
@@ -359,7 +358,7 @@ class CoverageSystem {
 
   void ComputeVoronoiCells() {
     UpdateRobotPositions();
-    voronoi_ = Voronoi(robot_global_positions_, GetWorldIDF(),
+    voronoi_ = Voronoi(robot_global_positions_, GetWorldMap(),
                        Point2(params_.pWorldMapSize, params_.pWorldMapSize),
                        params_.pResolution);
     voronoi_cells_ = voronoi_.GetVoronoiCells();
@@ -443,7 +442,7 @@ class CoverageSystem {
   //
   //! @{
   const auto &GetWorldIDFObject() const { return world_idf_; }
-  const MapType &GetWorldIDF() const { return world_idf_.GetWorldMap(); }
+  const MapType &GetWorldMap() const { return world_idf_.GetWorldMap(); }
   const MapType &GetSystemMap() const { return system_map_; }
   const MapType &GetSystemExplorationMap() const { return exploration_map_; }
   const MapType &GetSystemExploredIDFMap() const { return explored_idf_map_; }
@@ -489,13 +488,15 @@ class CoverageSystem {
     return robot_global_positions_;
   }
 
-  Point2 GetRobotPosition(int const robot_id, bool force_no_noise = false) {
+  Point2 GetRobotPosition(int const robot_id,
+                          bool force_no_noise = false) const {
     Point2 robot_pos;
     robot_pos[0] = robots_[robot_id].GetGlobalCurrentPosition()[0];
     robot_pos[1] = robots_[robot_id].GetGlobalCurrentPosition()[1];
     if (params_.pAddNoisePositions and not force_no_noise) {
       return AddNoise(robot_pos);
     }
+    return robot_pos;
   }
 
   const MapType &GetRobotLocalMap(size_t const id) {
diff --git a/cppsrc/core/include/CoverageControl/parameters.h b/cppsrc/core/include/CoverageControl/parameters.h
@@ -161,7 +161,7 @@ class Parameters {
   explicit Parameters(std::string const &config_file)
       : config_file_{config_file} {
     ParseParameters();
-    PrintParameters();
+    /* PrintParameters(); */
   }
 
   void SetConfig(std::string const &config_file) {
diff --git a/cppsrc/core/include/CoverageControl/world_idf.h b/cppsrc/core/include/CoverageControl/world_idf.h
@@ -75,7 +75,7 @@ class WorldIDF {
   /*! Fills in values of the world_map_ with the total importance for each cell
    */
   void GenerateMapCPU() {
-    std::cout << "Generating map using CPU" << std::endl;
+    /* std::cout << "Generating map using CPU" << std::endl; */
     float max_importance = 0;
     for (int i = 0; i < params_.pWorldMapSize; ++i) {  // Row (x index)
       float x1 = params_.pResolution * i;   // Left x-coordinate of pixel
@@ -84,7 +84,7 @@ class WorldIDF {
         float y1 = params_.pResolution * j;   // Lower y-coordinate of pixel
         float y2 = y1 + params_.pResolution;  // Upper y-coordinate of pixel
         float importance =
-            ComputeImportanceRectangle(Point2(x1, y1), Point2(x2, y2));
+            ComputeImportanceBND(Point2(x1, y1), Point2(x2, y2));
         if (std::abs(importance) < kEps) {
           importance = 0;
         }
@@ -109,9 +109,31 @@ class WorldIDF {
     }
   }
 
+  /*! Integrate each normal distribution over a rectangle (cell).
+   *  If the cell is far away, the importance is set to 0
+   */
+  float ComputeImportanceBND(Point2 const &bottom_left,
+                                   Point2 const &top_right) const {
+    Point2 bottom_right(top_right.x(), bottom_left.y());
+    Point2 top_left(Point2(bottom_left.x(), top_right.y()));
+    float importance = 0;
+    for (auto const &normal_distribution : normal_distributions_) {
+      Point2 mid_point = (bottom_left + top_right) / 2.;
+      if (normal_distribution.TransformPoint(mid_point).squaredNorm() >
+          params_.pTruncationBND * params_.pTruncationBND) {
+        continue;
+      }
+      importance += normal_distribution.IntegrateQuarterPlaneF(bottom_left);
+      importance -= normal_distribution.IntegrateQuarterPlaneF(bottom_right);
+      importance -= normal_distribution.IntegrateQuarterPlaneF(top_left);
+      importance += normal_distribution.IntegrateQuarterPlaneF(top_right);
+    }
+    return importance;
+  }
+
 #ifdef COVERAGECONTROL_WITH_CUDA
   void GenerateMapCuda() {
-    std::cout << "Generating map using CUDA" << std::endl;
+    /* std::cout << "Generating map using CUDA" << std::endl; */
     GenerateMapCuda(static_cast<float>(params_.pResolution),
                     static_cast<float>(params_.pTruncationBND),
                     static_cast<int>(params_.pWorldMapSize));
@@ -192,6 +214,7 @@ class WorldIDF {
 
   explicit WorldIDF(Parameters const &params) : WorldIDF(params.pWorldMapSize) {
     params_ = params;
+    GenerateMap();
   }
 
   WorldIDF(Parameters const &params, std::string const &file_name)
@@ -231,6 +254,29 @@ class WorldIDF {
         exit(1);
       }
     }
+    GenerateMap();
+  }
+
+  /*! Copy constructor to handle deep copy of the world map */
+  WorldIDF(WorldIDF const &other) {
+    world_map_ = other.world_map_;
+    params_ = other.params_;
+    normalization_factor_ = other.normalization_factor_;
+    is_cuda_available_ = other.is_cuda_available_;
+    normal_distributions_ = other.normal_distributions_;
+    polygon_features_ = other.polygon_features_;
+  }
+
+  WorldIDF &operator=(WorldIDF const &other) {
+    if (this != &other) {
+      world_map_ = other.world_map_;
+      params_ = other.params_;
+      normalization_factor_ = other.normalization_factor_;
+      is_cuda_available_ = other.is_cuda_available_;
+      normal_distributions_ = other.normal_distributions_;
+      polygon_features_ = other.polygon_features_;
+    }
+    return *this;
   }
 
   void LoadMap(std::string const &file_name) {
@@ -265,28 +311,6 @@ class WorldIDF {
     }
   }
 
-  /*! Integrate each normal distribution over a rectangle (cell).
-   *  If the cell is far away, the importance is set to 0
-   */
-  float ComputeImportanceRectangle(Point2 const &bottom_left,
-                                   Point2 const &top_right) const {
-    Point2 bottom_right(top_right.x(), bottom_left.y());
-    Point2 top_left(Point2(bottom_left.x(), top_right.y()));
-    float importance = 0;
-    for (auto const &normal_distribution : normal_distributions_) {
-      Point2 mid_point = (bottom_left + top_right) / 2.;
-      if (normal_distribution.TransformPoint(mid_point).squaredNorm() >
-          params_.pTruncationBND * params_.pTruncationBND) {
-        continue;
-      }
-      importance += normal_distribution.IntegrateQuarterPlane(bottom_left);
-      importance -= normal_distribution.IntegrateQuarterPlane(bottom_right);
-      importance -= normal_distribution.IntegrateQuarterPlane(top_left);
-      importance += normal_distribution.IntegrateQuarterPlane(top_right);
-    }
-    return importance;
-  }
-
   void GenerateMap() {
 #ifdef COVERAGECONTROL_WITH_CUDA
     /* GenerateMapCuda(); */
diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp
diff --git a/cppsrc/main/CMakeLists.txt b/cppsrc/main/CMakeLists.txt
diff --git a/cppsrc/main/coverage_algorithm.cpp b/cppsrc/main/coverage_algorithm.cpp
diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h
diff --git a/cppsrc/python_bindings/python_binds.cpp b/cppsrc/python_bindings/python_binds.cpp

Original file line number	Diff line number	Diff line change
`@@ -105,7 +105,7 @@ class CentralizedCVT : public AbstractController {`
`105`	`105`
`106`	`106`	`bool IsConverged() const { return is_converged_; }`
`107`	`107`
`108`		`- bool ComputeActions() {`
	`108`	`+ int ComputeActions() {`
`109`	`109`	`is_converged_ = true;`
`110`	`110`	`robot_global_positions_ = env_.GetRobotPositions();`
`111`	`111`	`ComputeGoals();`
`@@ -131,7 +131,7 @@ class CentralizedCVT : public AbstractController {`
`131`	`131`	`actions_[iRobot] = speed * direction;`
`132`	`132`	`is_converged_ = false;`
`133`	`133`	`}`
`134`		`- return true;`
	`134`	`+ return 0;`
`135`	`135`	`}`
`136`	`136`	`};`
`137`	`137`
Original file line number	Diff line number	Diff line change
`@@ -80,7 +80,7 @@ class ClairvoyantCVT : public AbstractController {`
`80`	`80`	`actions_.resize(num_robots_);`
`81`	`81`	`goals_ = robot_global_positions_;`
`82`	`82`	`voronoi_mass_.resize(num_robots_, 0);`
`83`		`- voronoi_ = Voronoi(robot_global_positions_, env_.GetWorldIDF(),`
	`83`	`+ voronoi_ = Voronoi(robot_global_positions_, env_.GetWorldMap(),`
`84`	`84`	`Point2(params_.pWorldMapSize, params_.pWorldMapSize),`
`85`	`85`	`params_.pResolution);`
`86`	`86`	`ComputeGoals();`
`@@ -101,7 +101,7 @@ class ClairvoyantCVT : public AbstractController {`
`101`	`101`	`}`
`102`	`102`	`}`
`103`	`103`
`104`		`- bool ComputeActions() {`
	`104`	`+ int ComputeActions() {`
`105`	`105`	`is_converged_ = true;`
`106`	`106`	`robot_global_positions_ = env_.GetRobotPositions();`
`107`	`107`	`ComputeGoals();`
`@@ -127,7 +127,7 @@ class ClairvoyantCVT : public AbstractController {`
`127`	`127`	`actions_[iRobot] = speed * direction;`
`128`	`128`	`is_converged_ = false;`
`129`	`129`	`}`
`130`		`- return true;`
	`130`	`+ return 0;`
`131`	`131`	`}`
`132`	`132`
`133`	`133`	`bool IsConverged() const { return is_converged_; }`
Original file line number	Diff line number	Diff line change
`@@ -147,7 +147,7 @@ class DecentralizedCVT : public AbstractController {`
`147`	`147`	`}`
`148`	`148`	`}`
`149`	`149`
`150`		`- bool ComputeActions() {`
	`150`	`+ int ComputeActions() {`
`151`	`151`	`is_converged_ = true;`
`152`	`152`	`robot_global_positions_ = env_.GetRobotPositions();`
`153`	`153`	`ComputeGoals();`
`@@ -173,8 +173,8 @@ class DecentralizedCVT : public AbstractController {`
`173`	`173`	`direction.normalize();`
`174`	`174`	`actions_[iRobot] = speed * direction;`
`175`	`175`	`is_converged_ = false;`
`176`		`- return true;`
`177`	`176`	`}`
	`177`	`+ return 0;`
`178`	`178`	`}`
`179`	`179`
`180`	`180`	`bool IsConverged() const { return is_converged_; }`
Original file line number	Diff line number	Diff line change
`@@ -338,7 +338,7 @@ class OracleBangExploreExploit : public AbstractController {`
`338`	`338`	`/* } */`
`339`	`339`	`}`
`340`	`340`
`341`		`- bool ComputeActions() {`
	`341`	`+ int ComputeActions() {`
`342`	`342`	`if (first_step_ == true) {`
`343`	`343`	`first_step_ = false;`
`344`	`344`	`robot_global_positions_ = env_.GetRobotPositions();`
`@@ -368,7 +368,7 @@ class OracleBangExploreExploit : public AbstractController {`
`368`	`368`	`is_converged_ = false;`
`369`	`369`	`}`
`370`	`370`	`}`
`371`		`- return true;`
	`371`	`+ return 0;`
`372`	`372`	`}`
`373`	`373`
`374`	`374`	`bool IsConverged() { return is_converged_; }`
Original file line number	Diff line number	Diff line change
`@@ -159,7 +159,7 @@ class OracleExploreExploit : public AbstractController {`
`159`	`159`	`}`
`160`	`160`	`}`
`161`	`161`
`162`		`- bool ComputeActions() {`
	`162`	`+ int ComputeActions() {`
`163`	`163`	`if (start_exploit_ == false) {`
`164`	`164`	`if (recompute_goal_counter_ >= recompute_goal_steps_ or`
`165`	`165`	`is_converged_ == true) {`
`@@ -170,7 +170,7 @@ class OracleExploreExploit : public AbstractController {`
`170`	`170`	`}`
`171`	`171`	`is_converged_ = not env_.StepRobotsToGoals(goals_, actions_);`
`172`	`172`	`UpdateOracleMap();`
`173`		`- return true;`
	`173`	`+ return 0;`
`174`	`174`	`}`
`175`	`175`
`176`	`176`	`bool IsConverged() { return is_converged_; }`
Original file line number	Diff line number	Diff line change
`@@ -334,7 +334,7 @@ class OracleSimulExploreExploit {`
`334`	`334`	`* std::endl; */`
`335`	`335`	`}`
`336`	`336`
`337`		`- bool ComputeActions() {`
	`337`	`+ int ComputeActions() {`
`338`	`338`	`robot_global_positions_ = env_.GetRobotPositions();`
`339`	`339`	`goals_ = robot_global_positions_;`
`340`	`340`	`if (first_step_ == true) {`
`@@ -378,7 +378,7 @@ class OracleSimulExploreExploit {`
`378`	`378`	`}`
`379`	`379`	`is_converged_ = false;`
`380`	`380`	`}`
`381`		`- return true;`
	`381`	`+ return 0;`
`382`	`382`	`}`
`383`	`383`
`384`	`384`	`bool IsConverged() { return is_converged_; }`