diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py index d08d8ec5ba..bb9744ce10 100644 --- a/Mapping/grid_map_lib/grid_map_lib.py +++ b/Mapping/grid_map_lib/grid_map_lib.py @@ -62,7 +62,8 @@ 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 @@ -70,7 +71,8 @@ def get_value_from_xy_index(self, x_ind, y_ind): 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: diff --git a/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst index eaa876c80b..5c19680fbc 100644 --- a/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst +++ b/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 +++++++++++++ diff --git a/tests/test_grid_map_lib.py b/tests/test_grid_map_lib.py index 670b357ad3..19443e5648 100644 --- a/tests/test_grid_map_lib.py +++ b/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__)