Bounds Checking for SubGrid Iterator (#77)

Author: DLu

nav_grid_iterators/include/nav_grid_iterators/sub_grid.h

@@ -54,9 +54,6 @@ class SubGrid : public BaseIterator<SubGrid>
    * @param min_y Minimum index (y coordinate)
    * @param width Number of indexes in the x direction
    * @param height Number of indexes in the y direction
-   *
-   * @note No bounds checking is done for any of the coordinates. Assumes (min_x, min_y) and
-   * (min_x + width - 1, min_y + height - 1) are valid in the grid.
    */
   SubGrid(const nav_grid::NavGridInfo* info, unsigned int min_x, unsigned int min_y,
           unsigned int width, unsigned int height)
@@ -78,13 +75,18 @@ class SubGrid : public BaseIterator<SubGrid>
    * @param min_y Minimum index (y coordinate)
    * @param width Number of indexes in the x direction
    * @param height Number of indexes in the y direction
-   *
-   * @note No bounds checking is done for any of the coordinates. Assumes (min_x, min_y) and
-   * (min_x + width - 1, min_y + height - 1) are valid in the grid.
    */
   SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, unsigned int min_x, unsigned int min_y,
-          unsigned int width, unsigned int height)
-    : BaseIterator(info, index), min_x_(min_x), min_y_(min_y), width_(width), height_(height) {}
+          unsigned int width, unsigned int height);
+
+  /**
+   * @brief Public constructor using UIntBounds object that takes in an arbitrary index
+   * @param info NavGridInfo for the grid to iterate over
+   * @param index Initial index
+   * @param bounds UIntBounds
+   */
+  SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, const nav_core2::UIntBounds& bounds)
+    : SubGrid(info, index, bounds.getMinX(), bounds.getMinY(), bounds.getWidth(), bounds.getHeight()) {}
 
   /**@name Standard BaseIterator Interface */
   /**@{*/

nav_grid_iterators/src/sub_grid.cpp

@@ -36,6 +36,35 @@
 
 namespace nav_grid_iterators
 {
+SubGrid::SubGrid(const nav_grid::NavGridInfo* info, const nav_grid::Index& index,
+                 unsigned int min_x, unsigned int min_y,
+                 unsigned int width, unsigned int height)
+  : BaseIterator(info, index), min_x_(min_x), min_y_(min_y), width_(width), height_(height)
+{
+  // If the start coordinate is entirely off the grid or the size is 0
+  // we invalidate the entire iterator and give up immediately
+  if (min_x_ >= info->width || min_y_ >= info->height || width_ == 0 || height_ == 0)
+  {
+    index_ = nav_grid::Index(0, 0);
+    width_ = 0;
+    height_ = 0;
+    min_x_ = 0;
+    min_y_ = 0;
+    return;
+  }
+
+  // If the end coordinate is off the grid, we shorten the dimensions to
+  // cover the on-grid potion
+  if (min_x_ + width_ > info->width)
+  {
+    width_ = info->width - min_x_;
+  }
+  if (min_y_ + height_ > info->height)
+  {
+    height_ = info->height - min_y_;
+  }
+}
+
 SubGrid SubGrid::begin() const
 {
   return SubGrid(info_, min_x_, min_y_, width_, height_);

nav_grid_iterators/test/utest.cpp

@@ -117,6 +117,24 @@ TEST(SubGrid, sub_grid)
   ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, 1, 3, 2, 2)), 4);
   nav_core2::UIntBounds bounds(1, 3, 4, 3);
   ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 4);
+
+  // Empty Bounds
+  bounds.reset();
+  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 0);
+
+  // Partially Overlapping Bounds
+  bounds.touch(3, 2);
+  bounds.touch(6, 3);
+  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 4);
+
+  bounds.reset();
+  bounds.touch(1, 6);
+  bounds.touch(3, 9);
+  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 6);
+
+  // Different empty bounds
+  nav_core2::UIntBounds empty(1, 0, 0, 0);
+  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, empty)), 0);
 }
 
 TEST(SubGrid, equality)