[ROS 2][grid_map_core] Code enhancements from master b7293f5

grid_map_core/include/grid_map_core/BufferRegion.hpp

@@ -36,9 +36,7 @@ class BufferRegion
   constexpr static unsigned int nQuadrants = 4;
 
   BufferRegion();
-  BufferRegion(
-    const Index & startIndex, const Size & size,
-    const BufferRegion::Quadrant & quadrant);
+  BufferRegion(Index startIndex, Size size, BufferRegion::Quadrant quadrant);
   virtual ~BufferRegion() = default;
 
   const Index & getStartIndex() const;
@@ -50,7 +48,7 @@ class BufferRegion
 
 private:
   //! Start index (typically top-left) of the buffer region.
-  Index staretIndex_;
+  Index startIndex_;
 
   //! Size of the buffer region.
   Size size_;

grid_map_core/include/grid_map_core/CubicInterpolation.hpp

@@ -50,7 +50,7 @@ using FunctionValueMatrix = Eigen::Matrix4d;
  * @param[in] nElem - number of elements in the container
  * @return index that is within [0, nElem-1].
  */
-unsigned int bindIndexToRange(int idReq, unsigned int nElem);
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);
 
 
 /*!

grid_map_core/include/grid_map_core/TypeDefs.hpp

@@ -15,16 +15,16 @@
 namespace grid_map
 {
 
-typedef Eigen::MatrixXf Matrix;
-typedef Matrix::Scalar DataType;
-typedef Eigen::Vector2d Position;
-typedef Eigen::Vector2d Vector;
-typedef Eigen::Vector3d Position3;
-typedef Eigen::Vector3d Vector3;
-typedef Eigen::Array2i Index;
-typedef Eigen::Array2i Size;
-typedef Eigen::Array2d Length;
-typedef uint64_t Time;
+using Matrix = Eigen::MatrixXf;
+using DataType = Matrix::Scalar;
+using Position = Eigen::Vector2d;
+using Vector = Eigen::Vector2d;
+using Position3 = Eigen::Vector3d;
+using Vector3 = Eigen::Vector3d;
+using Index = Eigen::Array2i;
+using Size = Eigen::Array2i;
+using Length = Eigen::Array2d;
+using Time = uint64_t;
 
 /*
  * Interpolations are ordered in the order

grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp

@@ -1,7 +1,7 @@
 #ifndef GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORSPLUGIN_HPP_
 #define GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORSPLUGIN_HPP_
 
-#include <math.h>
+#include <cmath>
 
 template<typename Scalar>
 struct scalar_sum_of_finites_op

grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp

@@ -30,14 +30,14 @@ class CircleIterator
    * @param center the position of the circle center.
    * @param radius the radius of the circle.
    */
-  CircleIterator(const GridMap & gridMap, const Position & center, const double radius);
+  CircleIterator(const GridMap & gridMap, const Position & center, double radius);
 
   /*!
    * Assignment operator.
    * @param iterator the iterator to copy data from.
    * @return a reference to *this.
    */
-  CircleIterator & operator=(const CircleIterator & other);
+  CircleIterator & operator=(const CircleIterator & other) = default;
 
   /*!
    * Compare to another iterator.

grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp

@@ -33,14 +33,14 @@ class EllipseIterator
    */
   EllipseIterator(
     const GridMap & gridMap, const Position & center, const Length & length,
-    const double rotation = 0.0);
+    double rotation = 0.0);
 
   /*!
    * Assignment operator.
    * @param iterator the iterator to copy data from.
    * @return a reference to *this.
    */
-  EllipseIterator & operator=(const EllipseIterator & other);
+  EllipseIterator & operator=(const EllipseIterator & other) = default;
 
   /*!
    * Compare to another iterator.
@@ -88,7 +88,7 @@ class EllipseIterator
    * @param[out] bufferSize the buffer size of the submap.
    */
   void findSubmapParameters(
-    const Position & center, const Length & length, const double rotation,
+    const Position & center, const Length & length, double rotation,
     Index & startIndex, Size & bufferSize) const;
 
   //! Position of the circle center;

grid_map_core/src/BufferRegion.cpp

@@ -11,29 +11,29 @@ namespace grid_map
 {
 
 BufferRegion::BufferRegion()
-: staretIndex_(Index::Zero()),
+: startIndex_(Index::Zero()),
   size_(Size::Zero()),
   quadrant_(BufferRegion::Quadrant::Undefined)
 {
 }
 
 BufferRegion::BufferRegion(
-  const Index & index, const Size & size,
-  const BufferRegion::Quadrant & quadrant)
-: staretIndex_(index),
-  size_(size),
-  quadrant_(quadrant)
+  Index index, Size size,
+  BufferRegion::Quadrant quadrant)
+: startIndex_(std::move(index)),
+  size_(std::move(size)),
+  quadrant_(std::move(quadrant))
 {
 }
 
 const Index & BufferRegion::getStartIndex() const
 {
-  return staretIndex_;
+  return startIndex_;
 }
 
-void BufferRegion::setStartIndex(const Index & staretIndex)
+void BufferRegion::setStartIndex(const Index & startIndex)
 {
-  staretIndex_ = staretIndex;
+  startIndex_ = startIndex;
 }
 
 const Size & BufferRegion::getSize() const

grid_map_core/src/CubicInterpolation.cpp

@@ -15,18 +15,15 @@
 namespace grid_map
 {
 
-unsigned int bindIndexToRange(int idReq, unsigned int nElem)
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem)
 {
-  if (idReq < 0) {
-    return 0;
+  if (idReq >= nElem) {
+    return nElem - 1;
   }
-  if ((unsigned)idReq >= nElem) {
-    return static_cast<unsigned int>(nElem - 1);
-  }
-  return static_cast<unsigned int>(idReq);
+  return idReq;
 }
 
-double getLayerValue(const Matrix & layerMat, int rowReq, int colReq)
+double getLayerValue(const Matrix & layerMat, unsigned int rowReq, unsigned int colReq)
 {
   const auto numCol = layerMat.cols();
   const auto numRow = layerMat.rows();
@@ -96,7 +93,7 @@ bool assembleFunctionValueMatrix(
   }
 
   const Matrix & layerMatrix = gridMap.get(layer);
-  auto f = [&layerMatrix](int rowReq, int colReq) {
+  auto f = [&layerMatrix](unsigned int rowReq, unsigned int colReq) {
       double retVal = getLayerValue(layerMatrix, rowReq, colReq);
       return retVal;
     };
@@ -142,10 +139,7 @@ bool getIndicesOfMiddleKnot(
   const GridMap & gridMap, const Position & queriedPosition,
   Index * index)
 {
-  if (!gridMap.getIndex(queriedPosition, *index)) {
-    return false;
-  }
-  return true;
+  return gridMap.getIndex(queriedPosition, *index);
 }
 
 }  // namespace bicubic_conv
@@ -275,10 +269,7 @@ bool getClosestPointIndices(
   const GridMap & gridMap, const Position & queriedPosition,
   Index * index)
 {
-  if (!gridMap.getIndex(queriedPosition, *index)) {
-    return false;
-  }
-  return true;
+  return gridMap.getIndex(queriedPosition, *index);
 }
 
 bool computeNormalizedCoordinates(
@@ -307,8 +298,8 @@ bool getFunctionValues(const Matrix & layerData, const IndicesMatrix & indices,
 
 void bindIndicesToRange(const GridMap & gridMap, IndicesMatrix * indices)
 {
-  const int numCol = gridMap.getSize().y();
-  const int numRow = gridMap.getSize().x();
+  const unsigned int numCol = gridMap.getSize().y();
+  const unsigned int numRow = gridMap.getSize().x();
 
   // top left
   {
@@ -358,10 +349,11 @@ double firstOrderDerivativeAt(
   const Matrix & layerData, const Index & index, Dim2D dim,
   double resolution)
 {
-  const int numCol = layerData.cols();
-  const int numRow = layerData.rows();
+  const auto numCol{static_cast<unsigned int>(layerData.cols())};
+  const auto numRow{static_cast<unsigned int>(layerData.rows())};
 
-  double left, right;
+  double left;
+  double right;
   switch (dim) {
     case Dim2D::X: {
         left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y());
@@ -394,9 +386,8 @@ double mixedSecondOrderDerivativeAt(
    * the order doesn't matter. Derivative values are the same.
    * Taken from https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
    */
-
-  const int numCol = layerData.cols();
-  const int numRow = layerData.rows();
+  const auto numCol{static_cast<unsigned int>(layerData.cols())};
+  const auto numRow{static_cast<unsigned int>(layerData.rows())};
 
   const double f11 = layerData(
     bindIndexToRange(index.x() - 1, numRow),

grid_map_core/src/GridMap.cpp

@@ -8,7 +8,7 @@
 
 #include <Eigen/Dense>
 
-#include <math.h>
+#include <cmath>
 #include <algorithm>
 #include <cassert>
 #include <iostream>
@@ -82,17 +82,13 @@ const std::vector<std::string> & GridMap::getBasicLayers() const
 
 bool GridMap::hasBasicLayers() const
 {
-  return basicLayers_.size() > 0;
+  return !basicLayers_.empty();
 }
 
 bool GridMap::hasSameLayers(const GridMap & other) const
 {
-  for (const auto & layer : layers_) {
-    if (!other.exists(layer)) {
-      return false;
-    }
-  }
-  return true;
+  return std::all_of(layers_.begin(), layers_.end(),
+           [&](const std::string & layer){return other.exists(layer);});
 }
 
 void GridMap::add(const std::string & layer, const double value)
@@ -283,12 +279,8 @@ bool GridMap::isValid(const Index & index, const std::vector<std::string> & laye
   if (layers.empty()) {
     return false;
   }
-  for (const auto & layer : layers) {
-    if (!isValid(index, layer)) {
-      return false;
-    }
-  }
-  return true;
+  return std::all_of(layers.begin(), layers.end(),
+           [&](const std::string & layer){return isValid(index, layer);});
 }
 
 bool GridMap::getPosition3(
@@ -448,10 +440,10 @@ GridMap GridMap::getTransformedMap(
     if (sampleRatio > 0.0) {
       positionSamples.reserve(5);
       positionSamples.push_back(center);
-      positionSamples.push_back(Position3(center.x() - sampleLength, center.y(), center.z()));
-      positionSamples.push_back(Position3(center.x() + sampleLength, center.y(), center.z()));
-      positionSamples.push_back(Position3(center.x(), center.y() - sampleLength, center.z()));
-      positionSamples.push_back(Position3(center.x(), center.y() + sampleLength, center.z()));
+      positionSamples.emplace_back(center.x() - sampleLength, center.y(), center.z());
+      positionSamples.emplace_back(center.x() + sampleLength, center.y(), center.z());
+      positionSamples.emplace_back(center.x(), center.y() - sampleLength, center.z());
+      positionSamples.emplace_back(center.x(), center.y() + sampleLength, center.z());
     } else {
       positionSamples.push_back(center);
     }
@@ -507,10 +499,7 @@ bool GridMap::move(const Position & position, std::vector<BufferRegion> & newReg
       if (abs(indexShift(i)) >= getSize()(i)) {
         // Entire map is dropped.
         clearAll();
-        newRegions.push_back(
-          BufferRegion(
-            Index(0, 0), getSize(),
-            BufferRegion::Quadrant::Undefined));
+        newRegions.emplace_back(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined);
       } else {
         // Drop cells out of map.
         int sign = (indexShift(i) > 0 ? 1 : -1);
@@ -524,49 +513,43 @@ bool GridMap::move(const Position & position, std::vector<BufferRegion> & newReg
           // One region to drop.
           if (i == 0) {
             clearRows(index, nCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(index, 0), Size(nCells, getSize()(1)),
-                BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(index, 0), Size(nCells, getSize()(1)),
+              BufferRegion::Quadrant::Undefined);
           } else if (i == 1) {
             clearCols(index, nCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(0, index), Size(getSize()(0), nCells),
-                BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(0, index), Size(getSize()(0), nCells),
+              BufferRegion::Quadrant::Undefined);
           }
         } else {
           // Two regions to drop.
           int firstIndex = index;
           int firstNCells = getSize()(i) - firstIndex;
           if (i == 0) {
             clearRows(firstIndex, firstNCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(firstIndex, 0), Size(firstNCells, getSize()(1)),
-                BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(firstIndex, 0), Size(firstNCells, getSize()(1)),
+              BufferRegion::Quadrant::Undefined);
           } else if (i == 1) {
             clearCols(firstIndex, firstNCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(0, firstIndex), Size(getSize()(0), firstNCells),
-                BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(0, firstIndex), Size(getSize()(0), firstNCells),
+              BufferRegion::Quadrant::Undefined);
           }
 
           int secondIndex = 0;
           int secondNCells = nCells - firstNCells;
           if (i == 0) {
             clearRows(secondIndex, secondNCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(secondIndex, 0),
-                Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(secondIndex, 0), Size(secondNCells, getSize()(1)),
+              BufferRegion::Quadrant::Undefined);
           } else if (i == 1) {
             clearCols(secondIndex, secondNCells);
-            newRegions.push_back(
-              BufferRegion(
-                Index(0, secondIndex),
-                Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
+            newRegions.emplace_back(
+              Index(0, secondIndex), Size(getSize()(0), secondNCells),
+              BufferRegion::Quadrant::Undefined);
           }
         }
       }
@@ -579,7 +562,7 @@ bool GridMap::move(const Position & position, std::vector<BufferRegion> & newReg
   position_ += alignedPositionShift;
 
   // Check if map has been moved at all.
-  return indexShift.any() != 0;
+  return indexShift.any();
 }
 
 bool GridMap::move(const Position & position)