Theta star take two

.gitignore

@@ -14,6 +14,9 @@ build
 # Ignore the Visual Studio Code workspace directory
 .vscode/
 
+# ignore pycache files
+__pycache__/
+
 # Ignore local.properties file
 bin/
 local.properties

pathPlanner/bresenhams.py

@@ -1,3 +1,6 @@
+from numba import njit
+
+@njit(fastmath=True, cache=True)
 def get_line(start, end):
 
     """Bresenham's Line Algorithm

pathPlanner/node.py

@@ -1,5 +1,23 @@
 from point import point2d
 from typing import Optional, Union
+from numba import njit
+
+@njit(fastmath=True, cache=True)
+def jump(pos, dx, dy, grid):
+    """go in a direction until we are near a wall
+    inspired by JPS but more simple"""
+    if dx == 0 and dy == 0:
+        raise ValueError("dx and dy cannot be 0")
+    x, y = pos
+    while True:
+        if x < 0 or y < 0 or x >= grid.shape[0] or y >= grid.shape[1]:
+            return (x - dx, y - dy)
+        # calculate sum of all point around the current point
+        sum_ = grid[x-1:x+2, y-1:y+2].sum()
+        if sum_ != 0:
+            return (x, y)
+        x = x + dx
+        y = y + dy
 
 class node(point2d):
     def __init__(self, x:float, y:float, shortestDist:Optional[float]=float('inf'), parent:Optional['node']=None):
@@ -20,21 +38,23 @@ def set_heuristic_distance(self, goal:point2d):
             self.heuristic_distance = self.distance(goal)
         return self.heuristic_distance
 
-
-    def get_neighbors(self, diagonal:bool=True)->list:
+    def get_neighbors(self, diagonal:bool=True, grid=None)->list:
         neighbors:list = []
+        backupneighbors = []
         for i in range(-1, 1+1):
             for j in range(-1, 1+1):
-                if i == 0 and j == 0:
-                    continue
-                if (i==j) and not diagonal:
-                    continue
-
-                else:
-                    neighbors.append(node(self.x + i, self.y + j))
+                if not (i == 0 and j == 0):
+                    backupneighbors.append(node(self.x + i, self.y + j))
+                    jumppoint = jump((self.x, self.y), i, j, grid)
+                    if jumppoint != (self.x, self.y):
+                        neighbors.append(jumppoint)
 
         #neighbors = [neighbor for neighbor in neighbors if neighbor.x >= 0 and neighbor.y >= 0]
-
+        neighbors = [node(neighbor[0], neighbor[1], float('inf'), self) for neighbor in neighbors]
+        if len(neighbors) == 0:
+            neighbors = backupneighbors
+        # make sure neighbors are within the grid
+        # neighbors = [neighbor for neighbor in neighbors if neighbor.x >= 0 and neighbor.y >= 0 and neighbor.x < grid.shape[0] and neighbor.y < grid.shape[1]]
         return neighbors
 
     @property
@@ -43,3 +63,16 @@ def neighbors(self):
             self._neighbors = self.get_neighbors()
         return self._neighbors
 
+if __name__ == "__main__":
+    import numpy as np
+    import matplotlib.pyplot as plt
+    grid = np.zeros((10, 10))
+    grid[3:6, 3:6] = 1
+    start = node(0, 0)
+    goal = node(9, 9)
+    points = start.get_neighbors(diagonal=True, grid=grid)
+    print(points)
+    xs, ys = zip(*[(point.x, point.y) for point in points])
+    plt.scatter(xs, ys)
+    plt.imshow(grid, cmap='gray')
+    plt.show()

pathPlanner/point.py

@@ -1,5 +1,6 @@
 from typing import Optional, Union
 import bresenhams
+from numba import njit
 
 
 class point2d:
@@ -13,16 +14,23 @@ def distance(self, other: Union['point2d', tuple, list]) -> float:
 
     def as_tuple(self) -> tuple:
         return (self.x, self.y)
-
     def line_of_sight(self, other: 'point2d', grid) -> bool:
         """Returns true if there is a line of sight between self and other.
         """
         if self == other:
             return True
         if self is None or other is None:
             return False
+
+        if self.x < 0 or self.y < 0 or self.x >= grid.shape[0] or self.y >= grid.shape[1]:
+            return False
+        if other.x < 0 or other.y < 0 or other.x >= grid.shape[0] or other.y >= grid.shape[1]:
+            return False
+        if grid[self.x][self.y] == 1 or grid[other.x][other.y] == 1:
+            return False
+
         intersected_points = [point2d(x, y) for x, y in
-                              bresenhams.supercover(self.as_tuple(),
+                              bresenhams.get_line(self.as_tuple(),
                                                     other.as_tuple())]
         intersected_points.append(other)
         intersected_points.append(self)

pathPlanner/theta_star.py

@@ -1,11 +1,17 @@
+import time
+
 import point
 from node import node
 import numpy as np
+from numba import njit
 import bresenhams
+from time import perf_counter
 import traceback
+from queue import PriorityQueue
 from perlin_noise import PerlinNoise
 HERUISTIC_WEIGHT = 1
 
+
 def reconstruct_path(goal:node):
     path = []
     current = goal
@@ -47,15 +53,12 @@ def theta_star(start:node, goal:point, grid):
         if s == goal:
             return reconstruct_path(s)
         closedSet.append(s)
-        for neighbor in s.neighbors:
+        for neighbor in s.get_neighbors(diagonal=True, grid=grid):
             if neighbor in closedSet:
                 continue
-            #if neighbor.x >= len(grid) or neighbor.y >= len(grid[0]):
-            #    continue
-            if True:# grid[neighbor.x-1][neighbor.y-1] == 0:
-                update_vertex(s, neighbor, goal, grid)
-                if neighbor not in openSet and neighbor.parent != None:
-                    openSet.append(neighbor)
+            update_vertex(s, neighbor, goal, grid)
+            if neighbor not in openSet and neighbor.parent != None:
+                openSet.append(neighbor)
     return None
 
 
@@ -66,7 +69,7 @@ def theta_star(start:node, goal:point, grid):
     from cubicSpline import interpolate_xy
     WIDTH = 50
     HEIGHT = 50
-    FILL_PCT = 0.15
+    FILL_PCT = 0.35
     start = node(0, 0)
     goal = node(WIDTH-1, HEIGHT-1)
     grid = []
@@ -78,7 +81,7 @@ def theta_star(start:node, goal:point, grid):
         grid.append(arr)
 
     '''
-    noise = PerlinNoise(octaves=10)
+    noise = PerlinNoise(octaves=5)
     xpix, ypix = WIDTH, HEIGHT
     pic = [[noise([i/xpix, j/ypix]) for j in range(xpix)] for i in range(ypix)]
 
@@ -91,8 +94,12 @@ def theta_star(start:node, goal:point, grid):
     plt.show()
     grid[0][0] = 0
     grid[HEIGHT-1][WIDTH-1] = 0
+    theta_star(start, goal, grid)
 
+    start_time = time.perf_counter()
     path = theta_star(start, goal, grid)
+    end_time = time.perf_counter()
+    print("Time taken: ", (end_time - start_time) * 1000)
     print(path)
     splinex = []
     spliney = []

pathPlanner/theta_star_fast.py

@@ -0,0 +1,152 @@
+import numpy as np
+from numba import njit
+from numba import int32, float32, uint32
+from numba.experimental import jitclass
+from typing import Optional
+import bresenhams
+
+
+class Node:
+    def __init__(self, x, y, parent=None):
+        self.x: int = x
+        self.y: int = y
+        self.parent: Optional[Node] = parent
+        self.gScore: float = 0
+        self.heuristic: float = 0
+        self.f: float = 0
+
+    def neighbors(self):
+        _neighbors = []
+        for i in range(-1, 2):
+            for j in range(-1, 2):
+                if i == 0 and j == 0:
+                    continue
+                _neighbors.append(Node(self.x + i, self.y + j, self))
+        return _neighbors
+    def __eq__(self, other):
+        return self.x == other.x and self.y == other.y
+
+    @property
+    def pos(self):
+        return (self.x, self.y)
+
+    def __le__(self, other):
+        return self.f <= other.f
+
+    def __lt__(self, other):
+        return self.f < other.f
+
+    def __hash__(self):
+        return hash(self.pos)
+
+
+def line_of_sight(grid: np.ndarray, a, b) -> bool:
+    """Returns true if there is a line of sight between self and other.
+        """
+    if a == b:
+        return True
+    if a is None or b is None:
+        return False
+
+    if a.x < 0 or a.y < 0 or a.x >= grid.shape[0] or a.y >= grid.shape[1]:
+        return False
+    if b.x < 0 or b.y < 0 or b.x >= grid.shape[0] or b.y >= grid.shape[1]:
+        return False
+    if grid[a.x][a.y] == 1 or grid[b.x][b.y] == 1:
+        return False
+
+    intersected_points = [(x, y) for x, y in
+                          bresenhams.get_line(a.as_tuple(),
+                                              b.as_tuple())]
+    intersected_points.append(b)
+    intersected_points.append(a)
+
+    for point in intersected_points:
+        if point[0] < 0 or point[0] < 0 or point[1] >= grid.shape[
+            0] or point[1] >= grid.shape[1]:
+            return False
+        elif grid[point[0]][point[1]] == 1:
+            return False
+
+    return True
+
+def euclidian_node_distance(pose: Node, goal: Node):
+    return np.sqrt((pose.x - goal.x) ** 2 + (pose.y - goal.y) ** 2)
+
+
+def euclidian_tuple_distance(pose: tuple, goal: tuple):
+    return np.sqrt((pose[0] - goal[0]) ** 2 + (pose[1] - goal[1]) ** 2)
+
+
+def update_vertex(s: Node, neighbor: Node, grid: np.ndarray):
+    if line_of_sight(grid, s.parent, neighbor):
+        if s.gScore + euclidian_node_distance(s.parent, neighbor) < neighbor.gScore:
+            neighbor.gScore = s.parent.gScore + euclidian_node_distance(s, neighbor)
+            neighbor.f = neighbor.gScore + neighbor.heuristic
+            neighbor.parent = s.parent
+
+    elif (s.gScore + euclidian_node_distance(s, neighbor) < neighbor.gScore) \
+            and line_of_sight(grid, s, neighbor):
+        neighbor.gScore = s.gScore + euclidian_node_distance(s, neighbor)
+        neighbor.f = neighbor.gScore + neighbor.heuristic
+        neighbor.parent = s
+
+
+def theta_star(grid: np.ndarray, start: tuple, goal: tuple) -> Optional[
+    list]:
+    start_node = Node(start[0], start[1])
+    goal_node = Node(goal[0], goal[1])
+    start_node.heuristic = euclidian_node_distance(start_node, goal_node)
+    start_node.f = start_node.heuristic
+    open_set = [start_node]
+    closed_set = []
+    while open_set:
+        open_set.sort()
+        s = open_set.pop(0)
+        if s == goal_node:
+            path = []
+            while s.parent:
+                path.append(s.pos)
+                s = s.parent
+            path.append(s.pos)
+            return path[::-1]
+        closed_set.append(s)
+        for neighbor in s.neighbors():
+            if neighbor in closed_set or not grid[neighbor.x][
+                neighbor.y]:
+                continue
+            if neighbor not in open_set:
+                neighbor.heuristic = euclidian_node_distance(neighbor, goal_node)
+                neighbor.gScore = float('inf')
+                neighbor.f = float('inf')
+                neighbor.parent = None
+            update_vertex(s, neighbor, grid)
+            if neighbor not in open_set:
+                open_set.append(neighbor)
+    return None
+
+
+if __name__ == '__main__':
+    from perlin_noise import PerlinNoise
+    import matplotlib.pyplot as plt
+
+    noise = PerlinNoise(octaves=10, seed=1)
+    w, h = 10, 10
+    grid = np.zeros((w, h))
+    for i in range(w):
+        for j in range(h):
+            grid[i][j] = noise([i / w, j / h])
+    # threshold the grid
+    grid = np.where(grid > 1.5, 1, 0)
+    grid = np.zeros((w,h))
+
+    start = (0, 0)
+    goal = (w - 1, h - 1)
+    path = theta_star(grid, start, goal)
+    print(path)
+    plt.imshow(grid)
+    plt.show()
+    xs, ys = zip(*path)
+    plt.plot(ys, xs, 'r')
+    plt.show()
+