space-time A* skeleton almost finished

setup.py

@@ -0,0 +1,30 @@
+
+from distutils.core import setup
+
+setup(
+ name = 'space-time-astar', # How you named your package folder (MyLib)
+ packages = ['space-time-astar'], # Chose the same as "name"
+ version = '0.1', # Start with a small number and increase it with every change you make
+ license='MIT', # Chose a license from here: https://help.github.com/articles/licensing-a-repository
+ description = 'A* search algorithm with an added time dimension to deal with dynamic obstacles.', # Give a short description about your library
+ author = 'Haoran Peng', # Type in your name
+ author_email = '[email protected]', # Type in your E-Mail
+ url = 'https://github.com/GavinPHR/Space-Time-AStar', # Provide either the link to your github or to your website
+ download_url = 'https://github.com/user/reponame/archive/v_01.tar.gz', # I explain this later on
+ keywords = ['astar-algorithm', 'obstacle-avoidance', 'time-dimension'], # Keywords that define your package best
+ install_requires=[ # I get to this in a second
+ 'validators',
+ 'beautifulsoup4',
+ ],
+ classifiers=[
+ 'Development Status :: 3 - Alpha', # Chose either "3 - Alpha", "4 - Beta" or "5 - Production/Stable" as the current state of your package
+ 'Intended Audience :: Developers', # Define that your audience are developers
+ 'Topic :: Software Development :: Build Tools',
+ 'License :: OSI Approved :: MIT License', # Again, pick a license
+ 'Programming Language :: Python :: 3', #Specify which pyhton versions that you want to support
+ 'Programming Language :: Python :: 3.4',
+ 'Programming Language :: Python :: 3.5',
+ 'Programming Language :: Python :: 3.6',
+ 'Programming Language :: Python :: 3.7',
+ ],
+)

space-time-astar/__init__.py

@@ -0,0 +1 @@
+from space-time-astar.planner import Planner

space-time-astar/grid.py

@@ -25,15 +25,15 @@ def make_grid(grid_size: int, minx: int, maxx: int, miny: int, maxy: int) -> np.
  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)
+ grid = np.zeros([y_size, x_size, 2], dtype=np.int32)
  # 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])
+ grid[i][j] = np.array([x, y])
  return grid
 
  '''

space-time-astar/neighbour_table.py

@@ -6,17 +6,21 @@
 import numpy as np
 
 class NeighbourTable:
+ # Current Right Left Down Up
+ # directions = [(0, 0), (1, 0), (-1, 0), (0, 1), (0, -1)]
+ # Uncomment the line below for 9 directions, this might slow things down a lot
+ directions = [(0, 0), (1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)]
 
  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):
+ for dx, dy in self.directions:
  y, x = i + dy, j + dx,
  if x >= 0 and x < dimx and y >= 0 and y < dimy:
- neighbours.append([y, x])
+ neighbours.append(grid[y][x])
  table[self.hash(grid[i][j])] = np.array(neighbours)
  self.table = table
 
@@ -26,4 +30,13 @@ def lookup(self, position: np.ndarray) -> np.ndarray:
  @staticmethod
  def hash(grid_pos: np.ndarray) -> int:
  concat = str(grid_pos[0]) + str(grid_pos[1])
- return int(concat)
+ return concat
+
+if __name__ == '__main__':
+ grid = np.array([[[15,5],[15,6],[15,7],[15,8],[15,9]],
+ [[16,5],[16,6],[16,7],[16,8],[16,9]],
+ [[17,5],[17,6],[17,7],[17,8],[17,9]],
+ [[18,5],[18,6],[18,7],[18,8],[18,9]],
+ [[19,5],[19,6],[19,7],[19,8],[19,9]]])
+ nt = NeighbourTable(grid)
+ print(nt.lookup(np.array([16,7])))

space-time-astar/planner.py

@@ -4,14 +4,13 @@
 Email: [email protected]
 '''
 from typing import Tuple, List, Dict
-from collections import defaultdict
-import heapq
+from heapq import heappush, heappop
 import numpy as np
 from scipy.spatial import KDTree
 
 from neighbour_table import NeighbourTable
 from grid import Grid
-from copy import deepcopy
+from state import State
 
 
 # static obstacles must include the boundary
@@ -24,11 +23,11 @@ def __init__(self, grid_size: int,
  start: Tuple[int, int],
  goal: Tuple[int, int],
  static_obstacles: List[Tuple[int, int]],
- dynamic_obstacles: Dict[int, List[Tuple[int, int]]]):
+ dynamic_obstacles: Dict[int, List[Tuple[int, int]]],
+ max_iter:int = 10000,
+ debug = False):
  self.grid_size = grid_size
  self.robot_radius = robot_radius
- self.start = np.array(start)
- self.goal = np.array(goal)
  np_static_obstacles = np.array(static_obstacles)
  self.static_obstacles = KDTree(np_static_obstacles)
  self.dynamic_obstacles = dict((k, np.array(v)) for k, v in dynamic_obstacles.items())
@@ -37,64 +36,96 @@ def __init__(self, grid_size: int,
  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)
- # Function to hash a position
- self.hash = NeighbourTable.hash
+
+ self.start = self.grid.snap_to_grid(np.array(start))
+ self.goal = self.grid.snap_to_grid(np.array(goal))
+ self.max_iter = max_iter
+ self.debug = debug
 
  '''
- Used to calculate distance between two points
- Also an admissible and consistent heuristic for A*
+ An admissible and consistent heuristic for A*
  '''
  @staticmethod
  def h(start: np.ndarray, goal: np.ndarray) -> int:
- return int(np.linalg.norm(start-goal, 1)) # L2 norm
+ return int(np.linalg.norm(start-goal, 1)) # L1 norm
+
+ @staticmethod
+ def l2(start: np.ndarray, goal: np.ndarray) -> int:
+ return int(np.linalg.norm(start-goal, 2)) # L2 norm
 
  '''
  Check whether the nearest static obstacle is within radius
  '''
  def safe_static(self, grid_pos: np.ndarray) -> bool:
- return self.h(grid_pos, self.static_obstacles.query(grid_pos)) > self.robot_radius
+ return self.l2(grid_pos, self.static_obstacles.query(grid_pos)) > self.robot_radius
 
  '''
  Assume dynamic obstacles are agents with same radius, distance needs to be 2*radius
  '''
  def safe_dynamic(self, grid_pos: np.ndarray, time: int) -> bool:
- return all(self.h(grid_pos, obstacle) > 2*self.robot_radius
+ return all(self.l2(grid_pos, obstacle) > 2*self.robot_radius
  for obstacle in self.dynamic_obstacles.setdefault(time, []))
 
- '''
- Reconstruct path from A* search result
- '''
- def reconstruct_path(self, came_from: Dict[int, np.ndarray], current: np.ndarray):
- total_path = [current]
- while self.hash(current) in came_from.keys():
- current = came_from[self.hash(current)]
- total_path.append(current)
- return total_path[::-1]
-
  '''
  Space-Time A*
  '''
- def plan(self):
- # Standard A* setup, no surprise here
- '''
- need to implement heap here
- '''
- open_set = []
+ def plan(self) -> np.ndarray:
+ # Initialize the start state
+ s = State(self.start, 0, 0, self.h(self.start, self.goal))
+
+ open_set = [s]
+ closed_set = set()
+
+ # Keep track of parent nodes for reconstruction
  came_from = dict()
- g_score = defaultdict(lambda: float('inf'))
- g_score[self.hash(self.start)] = 0
- f_score = defaultdict(lambda: float('inf'))
- f_score[self.hash(self.start)] = self.h(self.start, self.goal)
 
- while open_set:
+ iter_ = 0
+ while open_set and iter_ < self.max_iter:
+ iter_ += 1
+ current_state = open_set[0] # Smallest element in min-heap
+ if current_state.pos_equal_to(self.goal):
+ if self.debug:
+ print('Path found after {0} iterations'.format(iter_))
+ return self.reconstruct_path(came_from, current_state)
+
+ closed_set.add(heappop(open_set))
+ epoch = current_state.time + 1
+ for neighbour in self.neighbour_table.lookup(current_state.pos):
+ neighbour_state = State(neighbour, epoch, current_state.g_score + 1, self.h(neighbour, self.goal))
+ # Check if visited
+ if neighbour_state in closed_set:
+ continue
+
+ # Avoid obstacles
+ if not self.safe_static(neighbour) or not self.safe_dynamic(neighbour, epoch):
+ continue
+
+ # Add to open set
+ if neighbour_state not in open_set:
+ came_from[neighbour_state] = current_state
+ heappush(open_set, neighbour_state)
+
+ if self.debug:
+ print('Open set is empty, no path found.')
+ return None
+
+ '''
+ Reconstruct path from A* search result
+ '''
+ def reconstruct_path(self, came_from: Dict[State, State], current: State) -> np.ndarray:
+ total_path = [current.pos]
+ while current in came_from.keys():
+ current = came_from[current]
+ total_path.append(current.pos)
+ return np.array(total_path[::-1])
 
 
 if __name__ == '__main__':
  grid_size = 1
  robot_radius = 2
- start = (3, 10)
- goal = (15, 20)
- static_obstacles = [(5, 15), (10, 20)]
+ start = (2, 2)
+ goal = (50, 50)
+ static_obstacles = [(0, 0), (60, 70)]
  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.neighbour_table.lookup(planner.grid.snap_to_grid([10,15])))
+ print(planner.plan())

space-time-astar/state.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python3
+'''
+Author: Haoran Peng
+Email: [email protected]
+'''
+import numpy as np
+
+class State:
+
+ def __init__(self, pos: np.ndarray, time: int, g_score: int, h_score: int):
+ self.pos = pos
+ self.time = time
+ self.g_score = g_score
+ self.f_score = g_score + h_score
+
+ def __hash__(self) -> int:
+ concat = str(self.pos[0]) + str(self.pos[1]) + '0' + str(self.time)
+ return int(concat)
+
+ def pos_equal_to(self, pos: np.ndarray) -> bool:
+ return np.array_equal(self.pos, pos)
+
+ def __lt__(self, other: 'State') -> bool:
+ return self.f_score < other.f_score
+
+ def __eq__(self, other: 'State') -> bool:
+ return self.__hash__() == other.__hash__()
+
+ def __str__(self):
+ return 'State(pos=[' + str(self.pos[0]) + ', ' + str(self.pos[1]) + '], ' \
+ + 'time=' + str(self.time) + ', fscore=' + str(self.f_score) + ')'
+
+ def __repr__(self):
+ return self.__str__()