added grid processing and neighbour lookup table for fast access

space-time-astar/grid.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+'''
+Author: Haoran Peng
+Email: [email protected]
+'''
+from typing import Tuple
+import numpy as np
+
+class Grid:
+
+ def __init__(self, grid_size, static_obstacles):
+ self.grid_size = grid_size
+ self.minx, self.maxx, self.miny, self.maxy = self.calculate_boundaries(static_obstacles)
+ self.grid = self.make_grid(grid_size, self.minx, self.maxx, self.miny, self.maxy)
+
+ @staticmethod
+ def calculate_boundaries(static_obstacles: np.ndarray) -> Tuple[int, int, int, int]:
+ min_ = np.min(static_obstacles, axis=0)
+ max_ = np.max(static_obstacles, axis=0)
+ return min_[0], max_[0], min_[1], max_[1]
+
+ @staticmethod
+ def make_grid(grid_size: int, minx: int, maxx: int, miny: int, maxy: int) -> np.ndarray:
+ # Calculate the size of the sides
+ x_size = (maxx - minx) // grid_size
+ y_size = (maxy - miny) // grid_size
+ # Initialize the grid, assuming grid is 2D
+ grid = np.zeros([y_size, x_size, 2], dtype=np.uint16)
+ # Fill the grid in
+ y = miny - grid_size / 2
+ for i in range(y_size):
+ y += grid_size
+ x = minx - grid_size / 2
+ for j in range(x_size):
+ x += grid_size
+ grid[i][j] = np.array([y, x])
+ return grid
+
+ '''
+ Snap an arbitrary position to the center of the grid
+ '''
+ def snap_to_grid(self, position: np.ndarray) -> np.ndarray:
+ i = (position[1] - self.miny) // self.grid_size
+ j = (position[0] - self.minx) // self.grid_size
+ if i >= len(self.grid):
+ i -= 1
+ if j >= len(self.grid[0]):
+ j -= 1
+ return self.grid[i][j]

space-time-astar/neighbour_table.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+'''
+Author: Haoran Peng
+Email: [email protected]
+'''
+import numpy as np
+
+class NeighbourTable:
+
+ def __init__(self, grid: np.ndarray):
+ dimy, dimx = len(grid), len(grid[0])
+ table = dict()
+ for i in range(dimy):
+ for j in range(dimx):
+ neighbours = []
+ for dx, dy in (1, 0), (-1, 0), (0, 1), (0, -1):
+ y, x = i + dy, j + dx,
+ if x >= 0 and x < dimx and y >= 0 and y < dimy:
+ neighbours.append([y, x])
+ table[self.hash(grid[i][j])] = np.array(neighbours)
+ self.table = table
+
+ def lookup(self, position: np.ndarray) -> np.ndarray:
+ return self.table[self.hash(position)]
+
+ @staticmethod
+ def hash(grid_pos: np.ndarray) -> int:
+ concat = str(grid_pos[0]) + str(grid_pos[1])
+ return int(concat)

space-time-astar/planner.py

@@ -6,6 +6,9 @@
 from typing import Tuple, List, Dict
 import numpy as np
 from scipy.spatial import KDTree
+
+from neighbour_table import NeighbourTable
+from grid import Grid
 from copy import deepcopy
 
 
@@ -28,35 +31,14 @@ def __init__(self, grid_size: int,
  self.static_obstacles = KDTree(np_static_obstacles)
  self.dynamic_obstacles = dict((k, np.array(v)) for k, v in dynamic_obstacles.items())
 
- self.minx, self.maxx, self.miny, self.maxy = self.calculate_boundaries(np_static_obstacles)
- self.grid = self.make_grid(grid_size, self.minx, self.maxx, self.miny, self.maxy)
-
-
- @staticmethod
- def calculate_boundaries(static_obstacles: np.ndarray) -> Tuple[int, int, int, int]:
- min_ = np.min(static_obstacles, axis=0)
- max_ = np.max(static_obstacles, axis=0)
- return min_[0], max_[0], min_[1], max_[1]
-
- @staticmethod
- def make_grid(grid_size: int, minx: int, maxx: int, miny: int, maxy: int) -> np.ndarray:
- # Calculate the size of the sides
- x_size = (maxx - minx)//grid_size
- y_size = (maxy - miny)//grid_size
- # Initialize the grid, assuming grid is 2D
- grid = np.zeros([y_size, x_size, 2])
- # Fill the grid in
- y = miny - grid_size//2
- for i in range(y_size):
- y += grid_size
- x = minx - grid_size//2
- for j in range(x_size):
- x += grid_size
- grid[i][j] = np.array([y, x])
- return grid
+ # Make the grid according to the grid size
+ self.grid = Grid(grid_size, np_static_obstacles)
+ # Make a lookup table for looking up neighbours of a grid
+ self.neighbour_table = NeighbourTable(self.grid.grid)
 
 
 
+
 if __name__ == '__main__':
  grid_size = 1
  robot_radius = 2
@@ -65,4 +47,4 @@ def make_grid(grid_size: int, minx: int, maxx: int, miny: int, maxy: int) -> np.
  static_obstacles = [(5, 15), (10, 20)]
  dynamic_obstacles = {0: [(5, 16)], 1: [(5, 17)], 2: [(5, 18), (11, 20)]}
  planner = Planner(grid_size, robot_radius, start, goal, static_obstacles, dynamic_obstacles)
- print(planner.grid)
+ print(planner.neighbour_table.lookup(planner.grid.snap_to_grid([10,15])))