Added new sliding window iterator incl. unit tests and iterator demo.…

… Added new sliding window math expression filter.

grid_map_core/CMakeLists.txt

@@ -66,6 +66,7 @@ add_library(${PROJECT_NAME}
    src/iterators/SpiralIterator.cpp
    src/iterators/PolygonIterator.cpp
    src/iterators/LineIterator.cpp
+   src/iterators/SlidingWindowIterator.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -115,7 +116,9 @@ catkin_add_gtest(${PROJECT_NAME}-test
   test/PolygonIteratorTest.cpp
   test/PolygonTest.cpp
   test/EigenPluginsTest.cpp
-  test/SpiralIteratorTest.cpp)
+  test/SpiralIteratorTest.cpp
+  test/SlidingWindowIteratorTest.cpp
+  )
 endif()
 
 if(TARGET ${PROJECT_NAME}-test)

grid_map_core/include/grid_map_core/GridMap.hpp

@@ -362,11 +362,6 @@ class GridMap
    */
   bool extendToInclude(const GridMap& other);
 
-  /*!
-   * Rearranges data such that the buffer start index is at (0,0).
-   */
-  void convertToDefaultStartIndex();
-
   /*!
    * Clears all cells (set to NAN) for a layer.
    * @param layer the layer to be cleared.
@@ -452,6 +447,17 @@ class GridMap
    */
   const Index& getStartIndex() const;
 
+  /*!
+   * Checks if the buffer is at start index (0,0).
+   * @return true if buffer is at default start index.
+   */
+  bool isDefaultStartIndex() const;
+
+  /*!
+   * Rearranges data such that the buffer start index is at (0,0).
+   */
+  void convertToDefaultStartIndex();
+
  private:
 
   /*!

grid_map_core/include/grid_map_core/GridMapMath.hpp

@@ -108,33 +108,50 @@ bool getPositionShiftFromIndexShift(Vector& positionShift,
  * @param[in] bufferSize the size of the buffer.
  * @return true if index is within, and false if index is outside of the buffer.
  */
-bool checkIfIndexWithinRange(const Index& index, const Size& bufferSize);
+bool checkIfIndexInRange(const Index& index, const Size& bufferSize);
 
 /*!
- * Maps an index that runs out of the range of the circular buffer back into allowed the region.
- * This is the 2d version of mapIndexWithinRange(int&, const int&).
- * @param[in/out] index the indeces that will be mapped into the valid region of the buffer.
+ * Bounds an index that runs out of the range of the buffer.
+ * This means that an index that overflows is stopped at the last valid index.
+ * This is the 2d version of boundIndexToRange(int&, const int&).
+ * @param[in/out] index the indeces that will be bounded to the valid region of the buffer.
  * @param[in] bufferSize the size of the buffer.
  */
-void mapIndexWithinRange(Index& index,
-                         const Size& bufferSize);
+void boundIndexToRange(Index& index, const Size& bufferSize);
 
 /*!
- * Maps an index that runs out of the range of the circular buffer back into allowed the region.
- * @param[in/out] index the index that will be mapped into the valid region of the buffer.
+ * Bounds an index that runs out of the range of the buffer.
+ * This means that an index that overflows is stopped at the last valid index.
+ * @param[in/out] index the index that will be bounded to the valid region of the buffer.
  * @param[in] bufferSize the size of the buffer.
  */
-void mapIndexWithinRange(int& index, const int& bufferSize);
+void boundIndexToRange(int& index, const int& bufferSize);
 
 /*!
- * Limits (cuts off) the position to lie inside the map.
- * @param[in/out] position the position to be limited.
+ * Wraps an index that runs out of the range of the buffer back into allowed the region.
+ * This means that an index that overflows is reset to zero.
+ * This is the 2d version of wrapIndexToRange(int&, const int&).
+ * @param[in/out] index the indeces that will be wrapped into the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void wrapIndexToRange(Index& index, const Size& bufferSize);
+
+/*!
+ * Wraps an index that runs out of the range of the buffer back into allowed the region.
+ * This means that an index that overflows is reset to zero.
+ * @param[in/out] index the index that will be wrapped into the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void wrapIndexToRange(int& index, const int& bufferSize);
+
+/*!
+ * Bound (cuts off) the position to lie inside the map.
+ * This means that an index that overflows is stopped at the last valid index.
+ * @param[in/out] position the position to be bounded.
  * @param[in] mapLength the lengths in x and y direction.
  * @param[in] mapPosition the position of the map.
  */
-void limitPositionToRange(Position& position,
-                          const Length& mapLength,
-                          const Position& mapPosition);
+void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition);
 
 /*!
  * Provides the alignment transformation from the buffer order (outer/inner storage)

grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp

@@ -86,7 +86,7 @@ class GridMapIterator
    */
   bool isPastEnd() const;
 
-private:
+protected:
 
   //! Size of the buffer.
   Size size_;

grid_map_core/include/grid_map_core/iterators/iterators.hpp

@@ -15,3 +15,4 @@
 #include "grid_map_core/iterators/SpiralIterator.hpp"
 #include "grid_map_core/iterators/LineIterator.hpp"
 #include "grid_map_core/iterators/PolygonIterator.hpp"
+#include "grid_map_core/iterators/SlidingWindowIterator.hpp"

grid_map_core/src/GridMap.cpp

@@ -365,7 +365,7 @@ bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegio
         int endIndex = startIndex - sign + indexShift(i);
         int nCells = abs(indexShift(i));
         int index = (sign > 0 ? startIndex : endIndex);
-        mapIndexWithinRange(index, getSize()(i));
+        wrapIndexToRange(index, getSize()(i));
 
         if (index + nCells <= getSize()(i)) {
           // One region to drop.
@@ -404,7 +404,7 @@ bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegio
 
   // Update information.
   startIndex_ += indexShift;
-  mapIndexWithinRange(startIndex_, getSize());
+  wrapIndexToRange(startIndex_, getSize());
   position_ += alignedPositionShift;
 
   // Check if map has been moved at all.
@@ -574,9 +574,14 @@ const Index& GridMap::getStartIndex() const
   return startIndex_;
 }
 
+bool GridMap::isDefaultStartIndex() const
+{
+  return (startIndex_ == 0).all();
+}
+
 void GridMap::convertToDefaultStartIndex()
 {
-  if ((startIndex_ == 0).all()) return;
+  if (isDefaultStartIndex()) return;
 
   std::vector<BufferRegion> bufferRegions;
   if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {

grid_map_core/src/GridMapMath.cpp

@@ -107,7 +107,7 @@ bool getPositionFromIndex(Position& position,
                           const Size& bufferSize,
                           const Index& bufferStartIndex)
 {
-  if (!checkIfIndexWithinRange(index, bufferSize)) return false;
+  if (!checkIfIndexInRange(index, bufferSize)) return false;
   Vector offset;
   getVectorToFirstCell(offset, mapLength, resolution);
   position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
@@ -177,7 +177,7 @@ bool getPositionShiftFromIndexShift(Vector& positionShift,
   return true;
 }
 
-bool checkIfIndexWithinRange(const Index& index, const Size& bufferSize)
+bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
 {
   if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1])
   {
@@ -186,21 +186,33 @@ bool checkIfIndexWithinRange(const Index& index, const Size& bufferSize)
   return false;
 }
 
-void mapIndexWithinRange(Index& index,
-                         const Size& bufferSize)
+void boundIndexToRange(Index& index, const Size& bufferSize)
 {
   for (int i = 0; i < index.size(); i++) {
-    mapIndexWithinRange(index[i], bufferSize[i]);
+    boundIndexToRange(index[i], bufferSize[i]);
   }
 }
 
-void mapIndexWithinRange(int& index, const int& bufferSize)
+void boundIndexToRange(int& index, const int& bufferSize)
+{
+  if (index < 0) index = 0;
+  else if (index >= bufferSize) index = bufferSize - 1;
+}
+
+void wrapIndexToRange(Index& index, const Size& bufferSize)
+{
+  for (int i = 0; i < index.size(); i++) {
+    wrapIndexToRange(index[i], bufferSize[i]);
+  }
+}
+
+void wrapIndexToRange(int& index, const int& bufferSize)
 {
   if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize;
   index = index % bufferSize;
 }
 
-void limitPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
+void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
 {
   Vector vectorToOrigin;
   getVectorToOrigin(vectorToOrigin, mapLength);
@@ -248,13 +260,13 @@ bool getSubmapInformation(Index& submapTopLeftIndex,
 
   // Corners of submap.
   Position topLeftPosition = requestedSubmapPosition - transform * 0.5 * requestedSubmapLength.matrix();
-  limitPositionToRange(topLeftPosition, mapLength, mapPosition);
+  boundPositionToRange(topLeftPosition, mapLength, mapPosition);
   if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
   Index topLeftIndex;
   topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
 
   Position bottomRightPosition = requestedSubmapPosition + transform * 0.5 * requestedSubmapLength.matrix();
-  limitPositionToRange(bottomRightPosition, mapLength, mapPosition);
+  boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
   Index bottomRightIndex;
   if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
   bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
@@ -301,7 +313,7 @@ bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
   submapBufferRegions.clear();
 
   Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
-  mapIndexWithinRange(bottomRightIndex, bufferSize);
+  wrapIndexToRange(bottomRightIndex, bufferSize);
 
   BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
   BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
@@ -413,7 +425,7 @@ bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferSta
   }
 
   // End of iterations reached.
-  if (!checkIfIndexWithinRange(unwrappedIndex, bufferSize)) return false;
+  if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false;
 
   // Return true iterated index.
   index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
@@ -439,7 +451,7 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm
   }
 
   // End of iterations reached.
-  if (!checkIfIndexWithinRange(tempSubmapIndex, submapBufferSize)) return false;
+  if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
 
   // Get corresponding index in map.
   Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
@@ -456,7 +468,7 @@ Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
   if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex;
 
   Index index = bufferIndex - bufferStartIndex;
-  mapIndexWithinRange(index, bufferSize);
+  wrapIndexToRange(index, bufferSize);
   return index;
 }
 
@@ -465,7 +477,7 @@ Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const
   if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index;
 
   Index bufferIndex = index + bufferStartIndex;
-  mapIndexWithinRange(bufferIndex, bufferSize);
+  wrapIndexToRange(bufferIndex, bufferSize);
   return bufferIndex;
 }
 

grid_map_core/src/iterators/CircleIterator.cpp

@@ -84,8 +84,8 @@ void CircleIterator::findSubmapParameters(const Position& center, const double r
 {
   Position topLeft = center.array() + radius;
   Position bottomRight = center.array() - radius;
-  limitPositionToRange(topLeft, mapLength_, mapPosition_);
-  limitPositionToRange(bottomRight, mapLength_, mapPosition_);
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
   getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
   Index endIndex;
   getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);

grid_map_core/src/iterators/EllipseIterator.cpp

@@ -98,8 +98,8 @@ void EllipseIterator::findSubmapParameters(const Position& center, const Length&
   const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
   Position topLeft = center.array() + boundingBoxHalfLength;
   Position bottomRight = center.array() - boundingBoxHalfLength;
-  limitPositionToRange(topLeft, mapLength_, mapPosition_);
-  limitPositionToRange(bottomRight, mapLength_, mapPosition_);
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
   getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
   Index endIndex;
   getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);

grid_map_core/src/iterators/PolygonIterator.cpp

@@ -82,8 +82,8 @@ void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Ind
     topLeft = topLeft.array().max(vertex.array());
     bottomRight = bottomRight.array().min(vertex.array());
   }
-  limitPositionToRange(topLeft, mapLength_, mapPosition_);
-  limitPositionToRange(bottomRight, mapLength_, mapPosition_);
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
   getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
   Index endIndex;
   getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);

grid_map_core/src/iterators/SpiralIterator.cpp

@@ -28,7 +28,7 @@ SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Ve
   bufferSize_ = gridMap.getSize();
   gridMap.getIndex(center_, indexCenter_);
   nRings_ = std::ceil(radius_ / resolution_);
-  if (checkIfIndexWithinRange(indexCenter_, bufferSize_))
+  if (checkIfIndexInRange(indexCenter_, bufferSize_))
     pointsRing_.push_back(indexCenter_);
   else
     while(pointsRing_.empty() && !isPastEnd())
@@ -90,7 +90,7 @@ void SpiralIterator::generateRing()
   do {
     pointInMap.x() = point.x() + indexCenter_.x();
     pointInMap.y() = point.y() + indexCenter_.y();
-    if (checkIfIndexWithinRange(pointInMap, bufferSize_)) {
+    if (checkIfIndexInRange(pointInMap, bufferSize_)) {
       if (distance_ == nRings_ || distance_ == nRings_ - 1) {
         if (isInside(pointInMap))
           pointsRing_.push_back(pointInMap);