Fix GridMap cell indices wrapping across map edges

Mapping/grid_map_lib/grid_map_lib.py

@@ -62,15 +62,17 @@ def __init__(self, width, height, resolution,
     def get_value_from_xy_index(self, x_ind, y_ind):
         """get_value_from_xy_index
 
-        when the index is out of grid map area, return None
+        Return None if either index is outside the grid map area.
+        Valid indices satisfy 0 <= x_ind < width and 0 <= y_ind < height.
 
         :param x_ind: x index
         :param y_ind: y index
         """
 
         grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
 
-        if 0 <= grid_ind < self.n_data:
+        if (0 <= x_ind < self.width and 0 <= y_ind < self.height
+                and 0 <= grid_ind < self.n_data):
             return self.data[grid_ind]
         else:
             return None
@@ -111,6 +113,8 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):
         """set_value_from_xy_index
 
         return bool flag, which means setting value is succeeded or not
+        Return False without changing the map if either index is
+        outside 0 <= x_ind < width and 0 <= y_ind < height.
 
         :param x_ind: x index
         :param y_ind: y index
@@ -122,7 +126,9 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):
 
         grid_ind = int(y_ind * self.width + x_ind)
 
-        if 0 <= grid_ind < self.n_data and isinstance(val, self.data_type):
+        if (0 <= x_ind < self.width and 0 <= y_ind < self.height
+                and 0 <= grid_ind < self.n_data
+                and isinstance(val, self.data_type)):
             self.data[grid_ind] = val
             return True  # OK
         else:

docs/modules/5_path_planning/coverage_path/coverage_path_main.rst

@@ -8,6 +8,11 @@ This is a 2D grid based sweep coverage path planner simulation:
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif
 
+The underlying occupancy grid has finite boundaries. Cell indices range from
+zero to one less than the number of columns or rows. Cells outside either
+axis are treated as occupied by the sweep search, and obstacle expansion is
+clipped at the grid boundary rather than wrapping into another row.
+
 Code Link
 +++++++++++++
 

tests/test_grid_map_lib.py

@@ -1,6 +1,7 @@
-from Mapping.grid_map_lib.grid_map_lib import GridMap
+from Mapping.grid_map_lib.grid_map_lib import FloatGrid, GridMap
 import conftest
 import numpy as np
+import pytest
 
 
 def test_position_set():
@@ -36,5 +37,53 @@ def test_xy_and_grid_index_conversion():
             assert y_ind == y_ind_2
 
 
+@pytest.mark.parametrize("x_ind, y_ind", [
+    (-1, 1), (3, 0), (-3, 1), (6, 0), (3, -1), (-1, 2),
+    (0, -1), (0, 2),
+])
+def test_out_of_bounds_indices_do_not_alias_valid_cells(x_ind, y_ind):
+    grid_map = GridMap(3, 2, 0.5, 10.0, -0.5)
+    original = grid_map.data.copy()
+
+    assert grid_map.get_value_from_xy_index(x_ind, y_ind) is None
+    assert grid_map.check_occupied_from_xy_index(x_ind, y_ind, FloatGrid(1.0))
+    assert grid_map.set_value_from_xy_index(x_ind, y_ind, FloatGrid(1.0)) is False
+    assert grid_map.data == original
+
+
+@pytest.mark.parametrize("index_type", [int, float, np.int64])
+def test_valid_cell_indices_preserve_row_major_storage(index_type):
+    grid_map = GridMap(3, 2, 0.5, 10.0, -0.5)
+
+    for y_ind in range(grid_map.height):
+        for x_ind in range(grid_map.width):
+            value = FloatGrid(y_ind * grid_map.width + x_ind + 1.0)
+            assert grid_map.set_value_from_xy_index(
+                index_type(x_ind), index_type(y_ind), value) is True
+            assert grid_map.get_value_from_xy_index(
+                index_type(x_ind), index_type(y_ind)) is value
+
+    assert [value.get_float_data() for value in grid_map.data] == list(range(1, 7))
+
+
+@pytest.mark.parametrize("obstacle, expected", [
+    ((2, 0), {(2, 0), (1, 0), (2, 1)}),
+    ((0, 2), {(0, 2), (1, 2), (0, 1)}),
+])
+def test_expand_grid_does_not_wrap_across_edges(obstacle, expected):
+    grid_map = GridMap(3, 3, 1.0, 0.0, 0.0)
+    grid_map.set_value_from_xy_index(*obstacle, FloatGrid(1.0))
+
+    grid_map.expand_grid()
+
+    occupied = {
+        (x_ind, y_ind)
+        for y_ind in range(grid_map.height)
+        for x_ind in range(grid_map.width)
+        if grid_map.check_occupied_from_xy_index(x_ind, y_ind, FloatGrid(1.0))
+    }
+    assert occupied == expected
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)