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

grid_map_core/include/grid_map_core/BufferRegion.hpp

@@ -50,7 +50,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/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

@@ -37,7 +37,7 @@ class CircleIterator
    * @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

@@ -40,7 +40,7 @@ class EllipseIterator
    * @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.

grid_map_core/src/BufferRegion.cpp

@@ -11,7 +11,7 @@ namespace grid_map
 {
 
 BufferRegion::BufferRegion()
-: staretIndex_(Index::Zero()),
+: startIndex_(Index::Zero()),
   size_(Size::Zero()),
   quadrant_(BufferRegion::Quadrant::Undefined)
 {
@@ -20,20 +20,20 @@ BufferRegion::BufferRegion()
 BufferRegion::BufferRegion(
   const Index & index, const Size & size,
   const BufferRegion::Quadrant & quadrant)
-: staretIndex_(index),
+: startIndex_(index),
   size_(size),
   quadrant_(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

@@ -142,10 +142,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 +272,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(

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)

grid_map_core/src/iterators/CircleIterator.cpp

@@ -37,20 +37,6 @@ CircleIterator::CircleIterator(
   if (!isInside()) {++(*this);}
 }
 
-CircleIterator & CircleIterator::operator=(const CircleIterator & other)
-{
-  center_ = other.center_;
-  radius_ = other.radius_;
-  radiusSquare_ = other.radiusSquare_;
-  internalIterator_ = other.internalIterator_;
-  mapLength_ = other.mapLength_;
-  mapPosition_ = other.mapPosition_;
-  resolution_ = other.resolution_;
-  bufferSize_ = other.bufferSize_;
-  bufferStartIndex_ = other.bufferStartIndex_;
-  return *this;
-}
-
 bool CircleIterator::operator!=(const CircleIterator & other) const
 {
   return internalIterator_ != other.internalIterator_;

grid_map_core/src/iterators/EllipseIterator.cpp

@@ -6,7 +6,7 @@
  *   Institute: ETH Zurich, ANYbotics
  */
 
-#include <math.h>
+#include <cmath>
 #include <Eigen/Geometry>
 #include <memory>
 
@@ -23,8 +23,8 @@ EllipseIterator::EllipseIterator(
 : center_(center)
 {
   semiAxisSquare_ = (0.5 * length).square();
-  double sinRotation = sin(rotation);
-  double cosRotation = cos(rotation);
+  double sinRotation = std::sin(rotation);
+  double cosRotation = std::cos(rotation);
   transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
   mapLength_ = gridMap.getLength();
   mapPosition_ = gridMap.getPosition();
@@ -42,20 +42,6 @@ EllipseIterator::EllipseIterator(
   if (!isInside()) {++(*this);}
 }
 
-EllipseIterator & EllipseIterator::operator=(const EllipseIterator & other)
-{
-  center_ = other.center_;
-  semiAxisSquare_ = other.semiAxisSquare_;
-  transformMatrix_ = other.transformMatrix_;
-  internalIterator_ = other.internalIterator_;
-  mapLength_ = other.mapLength_;
-  mapPosition_ = other.mapPosition_;
-  resolution_ = other.resolution_;
-  bufferSize_ = other.bufferSize_;
-  bufferStartIndex_ = other.bufferStartIndex_;
-  return *this;
-}
-
 bool EllipseIterator::operator!=(const EllipseIterator & other) const
 {
   return internalIterator_ != other.internalIterator_;