Linting and Parity

cppsrc/core/CMakeLists.txt

@@ -7,7 +7,8 @@ FetchContent_Declare(CMakeExtraUtils
 FetchContent_MakeAvailable(CMakeExtraUtils)
 
 include(DynamicVersion)
-dynamic_version(PROJECT_PREFIX "CoverageControl_" GIT_ARCHIVAL_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../.git_archival.txt")
+dynamic_version(PROJECT_PREFIX "CoverageControl_" GIT_ARCHIVAL_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../.git_archival.txt"
+  FALLBACK_VERSION 0.0.1)
 
 project(CoverageControl VERSION ${PROJECT_VERSION} LANGUAGES CXX)
 

cppsrc/core/include/CoverageControl/algorithms/abstract_controller.h

@@ -29,9 +29,9 @@
 #ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ABSTRACT_CONTROLLER_H_
 #define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ABSTRACT_CONTROLLER_H_
 
-#include "CoverageControl/typedefs.h"
-#include "CoverageControl/parameters.h"
 #include "CoverageControl/coverage_system.h"
+#include "CoverageControl/parameters.h"
+#include "CoverageControl/typedefs.h"
 
 namespace CoverageControl {
 

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

@@ -59,10 +59,10 @@ namespace CoverageControl {
  * \class NearOptimalCVT
  * @}
  * The algorithm has knowledge of the entire map in a centralized manner.
- * It runs several trials; for each trial, it generates random initial positions for the sites.
- * Then uses the CVT to compute optimal centroids.
- * It performs the Hungarian algorithm to assign robots to the centroids.
- * Finally, the trial with the minimum cost is selected.
+ * It runs several trials; for each trial, it generates random initial positions
+ *for the sites. Then uses the CVT to compute optimal centroids. It performs the
+ *Hungarian algorithm to assign robots to the centroids. Finally, the trial with
+ *the minimum cost is selected.
  **/
 class NearOptimalCVT : public AbstractController {
  private:

cppsrc/core/include/CoverageControl/bivariate_normal_distribution.h

@@ -139,14 +139,27 @@ class BivariateNormalDistribution {
   Point2 TransformPoint(Point2 const &in_point) const {
     if (is_circular_) {
       return Point2((in_point - mean_) / sigma_.x());
-    } else {
-      Point2 translated = in_point - mean_;
-      Point2 normalized(translated.x() / sigma_.x(),
-                        translated.y() / sigma_.y());
-      return Point2((normalized.x() - rho_ * normalized.y()) /
-                        (std::sqrt(1 - rho_ * rho_)),
-                    normalized.y());
     }
+    Point2 translated = in_point - mean_;
+    Point2 normalized(translated.x() / sigma_.x(), translated.y() / sigma_.y());
+    return Point2(
+        (normalized.x() - rho_ * normalized.y()) / (std::sqrt(1 - rho_ * rho_)),
+        normalized.y());
+  }
+
+  Point2f TransformPoint(Point2f const &in_point_f) const {
+    Point2f mean_f = mean_.cast<float>();
+    Point2f sigma_f = sigma_.cast<float>();
+    float rho_f = static_cast<float>(rho_);
+    if (is_circular_ or std::abs(rho_f) < kEpsf) {
+      return Point2f((in_point_f - mean_f) / sigma_f.x());
+    }
+    Point2f translated = in_point_f - mean_f;
+    Point2f normalized(translated.x() / sigma_f.x(),
+                       translated.y() / sigma_f.y());
+    return Point2f(
+        (normalized.x() - rho_f * normalized.y()) / (sqrt(1 - rho_f * rho_f)),
+        normalized.y());
   }
 
   /*!
@@ -158,18 +171,16 @@ class BivariateNormalDistribution {
    * @return Value of the integral
    */
   double IntegrateQuarterPlane(Point2 const &point) const {
-    auto transformed_point = TransformPoint(point);
+    Point2 transformed_point = TransformPoint(point);
     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;
+  float IntegrateQuarterPlane(Point2f const &point) const {
+    float scale_f = static_cast<float>(scale_);
+    Point2f transformed_point = TransformPoint(point);
+    return scale_f * std::erfc(transformed_point.x() * kOneBySqrt2f) *
+           std::erfc(transformed_point.y() * kOneBySqrt2f) / 4.f;
   }
 };
 

cppsrc/core/include/CoverageControl/constants.h

@@ -47,10 +47,11 @@ namespace CoverageControl {
  */
 
 double const kEps = 1e-10;          /*!< Epsilon for double comparison */
+float const kEpsf = 1e-6f;          /*!< Epsilon for float 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 */
+float const kOneBySqrt2f = 1.f / sqrtf(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 +

cppsrc/core/include/CoverageControl/coverage_system.h

@@ -38,7 +38,7 @@
 #include <fstream>
 #include <iostream>
 #include <list>
-/* #include <mutex> */
+#include <mutex>
 #include <random>
 #include <string>
 #include <utility>
@@ -78,7 +78,7 @@ class CoverageSystem {
   mutable std::random_device
       rd_;                    //!< Random device for random number generation
   mutable std::mt19937 gen_;  //!< Mersenne Twister random number generator
-  /* mutable std::mutex mutex_; */
+  mutable std::mutex mutex_;
   std::uniform_real_distribution<>
       distrib_pts_;  //!< Uniform distribution for generating random points
   PointVector robot_global_positions_;  //!< Global positions of the robots
@@ -95,8 +95,6 @@ class CoverageSystem {
       0;                         //!< Weighted ratio of explored locations
   double total_idf_weight_ = 0;  //!< Total weight of the world IDF
   std::vector<PlotterData> plotter_data_;  //!< Stores data for plotting
-  std::vector<std::vector<int>>
-      adjacency_matrix_;  //!< Adjacency matrix for communication
   std::vector<std::vector<Point2>>
       relative_positions_neighbors_;  //!< Relative positions of neighboring
                                       //!< robots for each robot
@@ -108,6 +106,7 @@ class CoverageSystem {
 
   //! Update the exploration map, explored IDF map, and system map
   void UpdateSystemMap() {
+    // This is not necessarily thread safe. Do NOT parallelize this for loop
     for (size_t i = 0; i < num_robots_; ++i) {
       MapUtils::MapBounds index, offset;
       MapUtils::ComputeOffsets(params_.pResolution, robot_global_positions_[i],

cppsrc/core/include/CoverageControl/cuda_utils.h

@@ -32,8 +32,8 @@
 #include <iostream>
 #include <limits>
 #include <sstream>
-#include <vector>
 #include <string>
+#include <vector>
 
 #include "CoverageControl/Config.h"
 namespace CoverageControl {
@@ -57,11 +57,6 @@ class CudaUtils {
    */
   CudaUtils() = delete;
 
-  /*!
-   * Destructor deleted as we don't want to create an instance of this class
-   */
-  ~CudaUtils() = delete;
-
   static bool UseCuda() { return use_cuda_; }
 
   static void SetUseCuda(bool use_cuda) { use_cuda_ = use_cuda; }

cppsrc/core/include/CoverageControl/plotter.h

@@ -33,8 +33,8 @@
 #include <filesystem>
 #include <iostream>
 #include <list>
-#include <vector>
 #include <string>
+#include <vector>
 
 #include "CoverageControl/typedefs.h"
 #include "CoverageControl/voronoi.h"

cppsrc/core/include/CoverageControl/robot_model.h

@@ -33,10 +33,10 @@
 #ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ROBOT_MODEL_H_
 #define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ROBOT_MODEL_H_
 
+#include <algorithm>
 #include <cmath>
 #include <iostream>
 #include <memory>
-#include <algorithm>
 #include <vector>
 
 #include "CoverageControl/constants.h"

cppsrc/core/include/CoverageControl/typedefs.h

@@ -41,8 +41,9 @@ namespace CoverageControl {
  * @{
  */
 
-typedef Eigen::Vector2d Point2; /*!< Point2 is a 2D vector of doubles */
-typedef Eigen::Vector3d Point3; /*!< Point3 is a 3D vector of doubles */
+typedef Eigen::Vector2d Point2;  /*!< Point2 is a 2D vector of doubles */
+typedef Eigen::Vector2f Point2f; /*!< Point2 is a 2D vector of doubles */
+typedef Eigen::Vector3d Point3;  /*!< Point3 is a 3D vector of doubles */
 typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
     MapType; /*!< MapType is a 2D matrix of floats. Note: It is RowMajor */