Now it's A_star

Motion Planning/first_search.py

@@ -15,6 +15,7 @@
 #   0 = Navigable space
 #   1 = Occupied space
 
+import heapq
 from collections import deque
 
 import numpy as np
@@ -32,7 +33,7 @@ grid = np.array([[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
                  [0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0]])
 init = np.array([0, 0])
 # goal = np.array([len(grid)-1, len(grid[0])-1])
-goal = np.array([7, 10])
+goal = np.array([0, 10])
 cost = 1
 
 delta = np.array([[-1, 0],  # go up
@@ -44,38 +45,67 @@ delta_name = ['^', '<', 'v', '>']
 
 
 def check_navigable(grid, goal) -> bool:
+    goal = np.array(goal)
     if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
-        if not grid[goal[0], goal[1]]:
+        if not grid[int(goal[0]), int(goal[1])]:
             return True
     return False
 
 
+def heuristic(now_pos, goal) -> float:
+    """Returns the Euclidian distance between two points
+
+    Parameters
+    ----------
+    now_pos : np.ndarray
+        The current position
+    goal : np.ndarray
+        The goal position
+
+    Returns
+    -------
+    float
+        Distance
+    """
+    if type(now_pos) is not np.ndarray:
+        now_pos = np.array(now_pos)
+    if type(goal) is not np.ndarray:
+        goal = np.array(goal)
+    return np.linalg.norm(now_pos-goal)
+
+
 def search(grid: np.ndarray, init: list, goal: list, cost: list):
     # ----------------------------------------
     # insert code here
     # ----------------------------------------
-    next_check = deque()
-    next_check.append(init)
+    next_check = []
+    heapq.heappush(next_check, np.concatenate(
+        ([heuristic(init, goal), 0], init)))
     already_checked = dict()
     already_checked[tuple(init)] = 0
-    cost_map = np.zeros([len(grid), len(grid[0])]) + float('INF')
+    cost_map = np.zeros([len(grid), len(grid[0])]) + float('inf')
     cost_map[init[0], init[1]] = 0
-    expand_map = np.zeros([len(grid), len(grid[0])]) - 1
-    expand_now = 0
-    expand_map[init[0], init[1]] = expand_now
+    expand_map = np.zeros([len(grid), len(grid[0])]) - float('inf')
+    current_expand_num = 0
+    expand_map[init[0], init[1]] = current_expand_num
 
     while next_check:
-        checking = next_check.popleft()
-        if check_navigable(grid, checking):
+        current_center_grid = heapq.heappop(next_check)
+        current_center_grid = current_center_grid[2:4]  # type: list[int]
+        if check_navigable(grid, current_center_grid):
             for move_num in range(len(delta)):
-                next = checking+delta[move_num]  # type: np.ndarray
+                next = current_center_grid+delta[move_num]  # type: np.ndarray
                 if check_navigable(grid, next) and tuple(next) not in already_checked:
-                    expand_now += 1
-                    next_check.append(next)
-                    cost_now = already_checked[checking[0], checking[1]]+cost
-                    already_checked[tuple(next)] = cost_now
-                    cost_map[next[0], next[1]] = cost_now
-                    expand_map[next[0], next[1]] = expand_now
+                    current_expand_num += 1
+                    gg = [heuristic(next, goal),
+                          current_expand_num] + list(next)
+                    heapq.heappush(next_check, gg)
+                    # next_check.append(next)
+                    current_cost = already_checked[current_center_grid[0],
+                                                   current_center_grid[1]]+cost
+                    already_checked[tuple(next)] = current_cost
+                    cost_map[int(next[0]), int(next[1])] = current_cost
+                    expand_map[int(next[0]), int(next[1])] = current_expand_num
                     if all(next == goal):
                         ic(cost_map)
                         ic(expand_map)