From be6da3a3460cbd9a70535e1ec8164ef3943fe0f5 Mon Sep 17 00:00:00 2001
From: Gavin Peng <gavinsweden@gmail.com>
Date: Wed, 11 Mar 2020 00:39:27 +0000
Subject: [PATCH] some decoupling and some progress on A*

---
 space-time-astar/planner.py | 52 ++++++++++++++++++++++++++++++++++++-
 1 file changed, 51 insertions(+), 1 deletion(-)

diff --git a/space-time-astar/planner.py b/space-time-astar/planner.py
index 7acb4e2..1279256 100644
--- a/space-time-astar/planner.py
+++ b/space-time-astar/planner.py
@@ -4,6 +4,8 @@
 Email: gavinsweden@gmail.com
 '''
 from typing import Tuple, List, Dict
+from collections import defaultdict
+import heapq
 import numpy as np
 from scipy.spatial import KDTree
 
@@ -35,8 +37,56 @@ 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
 
-    
+    '''
+    Used to calculate distance between two points
+    Also 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
+
+    '''
+    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
+
+    '''
+    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
+                   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 = []
+        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:
 
 
 if __name__ == '__main__':