[ROS 2][grid_map_core] Speed up submap iterator c8b1057

grid_map_core/src/GridMapMath.cpp

@@ -242,9 +242,24 @@ void wrapIndexToRange(Index & index, const Size & bufferSize)
 
 void wrapIndexToRange(int & index, int bufferSize)
 {
-  index = index % bufferSize;
-  if (index < 0) {
-    index += bufferSize;
+  // Try shortcuts before resorting to the expensive modulo operation.
+  if (index < bufferSize) {
+    if (index >= 0) {  // within the wanted range
+      return;
+      // Index is below range, but not more than one span of the range.
+    } else if (index >= -bufferSize) {
+      index += bufferSize;
+      return;
+    } else {  // Index is largely below range.
+      index = index % bufferSize;
+      index += bufferSize;
+    }
+    // Index is above range, but not more than one span of the range.
+  } else if (index < bufferSize * 2) {
+    index -= bufferSize;
+    return;
+  } else {  // Index is largely above range.
+    index = index % bufferSize;
   }
 }
 

grid_map_core/test/SubmapIteratorTest.cpp

@@ -165,3 +165,106 @@ TEST(SubmapIterator, CircularBuffer)
   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
   EXPECT_EQ(3, iterator.getSubmapIndex()(1));
 }
+
+/**
+ * The submap should contain the same elements as before even after moving the underlying map.
+ *
+ *                                                          +----------------------------+
+ *                                                          |                            |
+ *                                                          |                            |
+ *              +----------------------------+              |                            |
+ *              |0  0  0  0  0  0  0  0  0  0|              |      0  0  0  0  0  0  0  0|
+ *              |     +----+                 |              |           +----+           |
+ * Submap       |1  1 |1  1| 1  1  1  1  1  1|              |      1  1 |1  1| 1  1  1  1|
+ *           +------> |    |                 |              |           |    |           |
+ *              |2  2 |2  2| 2  2  2  2  2  2|              |      2  2 |2  2| 2  2  2  2|
+ *              |     +----+                 |              |           +----+           |
+ *              |3  3  3  3  3  3  3  3  3  3|   Move       |      3  3  3  3  3  3  3  3|
+ *              |                            |              |                            |
+ *              |4  4  4  4  4  4  4  4  4  4| +--------->  |      4  4  4  4  4  4  4  4|
+ *              |                            |              |                            |
+ *              |5  5  5  5  5  5  5  5  5  5|              |      5  5  5  5  5  5  5  5|
+ *              |                            |              |                            |
+ *              |6  6  6  6  6  6  6  6  6  6|              |      6  6  6  6  6  6  6  6|
+ *              |                            |              |                            |
+ *              |7  7  7  7  7  7  7  7  7  7|              |      7  7  7  7  7  7  7  7|
+ *              |                            |              +----------------------------+
+ *              |8  8  8  8  8  8  8  8  8  8|
+ *              |                            |
+ *              |9  9  9  9  9  9  9  9  9  9|
+ *              +----------------------------+
+ */
+TEST(SubmapIterator, InterleavedExecutionWithMove) {
+  grid_map::Index submapTopLeftIndex(3, 1);
+  grid_map::Size submapSize(2, 2);
+
+  grid_map::GridMap map({"layer"});
+
+  map.setGeometry(grid_map::Length(10, 10), 1.0, grid_map::Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  auto & layer = map.get("layer");
+
+  // Initialize the layer as sketched.
+  for (size_t colIndex = 0; colIndex < static_cast<size_t>(layer.cols()); colIndex++) {
+    layer.col(colIndex).setConstant(colIndex);
+  }
+
+  std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl;
+  // Instantiate the submap iterator as sketched.
+  grid_map::SubmapIterator iterator(map, submapTopLeftIndex, submapSize);
+
+  // check that the submap iterator returns {1,1,2,2}
+  auto checkCorrectValues = [](std::array<double, 4> given) {
+      int countOnes = 0, countTwos = 0;
+      for (auto & value : given) {
+        if (std::abs(value - 1.0) < 1e-6) {
+          countOnes++;
+        } else if (std::abs(value - 2.0) < 1e-6) {
+          countTwos++;
+        } else {
+          FAIL() << "Submap iterator returned unexpected value.";
+        }
+      }
+      EXPECT_EQ(countOnes, 2);
+      EXPECT_EQ(countTwos, 2);
+    };
+
+  std::array<double, 4> returnedSequence;
+  returnedSequence.fill(0);
+
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+
+  checkCorrectValues(returnedSequence);
+
+  // Reset the iterator and now check that it still returns the same sequence
+  // when we move the map interleaved with iterating.
+  iterator = grid_map::SubmapIterator(map, submapTopLeftIndex, submapSize);
+  returnedSequence.fill(0);
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    if (submapIndex == 2) {
+      // Now move the map as depicted.
+      map.move(grid_map::Position(2.0, 2.0));
+    }
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+  checkCorrectValues(returnedSequence);
+
+  // TODO(mwulf, mgaertner): This behavior is not yet implemented:
+  //
+  //  // Reset the iterator and now check that the iterator throws?
+  //  // if the submap moved out of range.
+  //  iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
+  //
+  //  EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+  //    if (submapIndex == 2) {
+  //      // Now move the map so that the submap gets out of range.
+  //      map.move(Position(20.0, 20.0));
+  //    }
+  //    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+  //    ++iterator;
+  //  });
+}