working a_star

a_star.py

@@ -1,14 +1,20 @@
 from models.House import House
 from math import fabs
 from models.Trash import Trash
+from config import MAP_HEIGHT, MAP_WIDTH, MAP
+from models.Road import Road
+import numpy as np
+from helpler import is_within_height, is_within_width
+import heapq
+import time
 
 
 class A_Star:
     TRASH_TYPES = ["mixed", "paper", "glass", "plastic"]
 
-    def __init__(self, draw_items, gc):
+    def __init__(self, draw_items, gc, env, refresh_screen):
         super().__init__()
-        self.draw_items, self.gc = draw_items, gc
+        self.draw_items, self.gc, self.env, self.refresh_screen = draw_items, gc, env, refresh_screen
         self.houses = list(map(lambda item: self.draw_items[item], list(filter(lambda item: isinstance(
             self.draw_items[item], House), self.draw_items))))
 
@@ -34,13 +40,12 @@ class A_Star:
 
     def best_move(self):
         houses, trashes = self.houses_with_trash()
+
         score = []
         for t_type in self.TRASH_TYPES:
-            if getattr(self.gc, t_type) == self.gc.limit:
+            if getattr(self.gc, t_type) == self.gc.limit or len(houses) == 0:
                 for trash in trashes:
-                    print(trash.trash_type, t_type)
-                    if trash.trash_type == t_type:
-                        print(trash.col, trash.row)
+                    if trash.trash_type == t_type and getattr(self.gc, t_type) != 0:
                         return (trash.col, trash.row)
 
         for house in houses:
@@ -53,3 +58,99 @@ class A_Star:
                           distance, "position": (house.col, house.row)})
 
         return sorted(score, key=lambda i: i['score'], reverse=True)[0]["position"]
+
+    def get_to_dest(self):
+        road_pos_array = np.zeros((MAP_HEIGHT, MAP_WIDTH))
+
+        for x, y in self.draw_items:
+            if MAP[y][x] == "Road":
+                road_pos_array[y, x] = 1
+
+        start = (self.gc.col, self.gc.row)
+
+        route = astar(array=road_pos_array, start=start,
+                      goal=self.best_move())
+
+        return route
+
+
+def astar(array, start, goal):
+    def heuristic(a, b):
+        return fabs((b[0] - a[0])) + fabs((b[1] - a[1]))
+
+    neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)]
+
+    close_set = set()
+
+    came_from = {}
+
+    gscore = {start: 0}
+
+    fscore = {start: heuristic(start, goal)}
+
+    oheap = []
+
+    heapq.heappush(oheap, (fscore[start], start))
+
+    while oheap:
+
+        current = heapq.heappop(oheap)[1]
+
+        if current[0] == goal[0] or current[1] == goal[1]:
+            if fabs(current[0] - goal[0]) == 1 or fabs(current[1] - goal[1]) == 1:
+                data = []
+
+                while current in came_from:
+
+                    data.append(current)
+
+                    current = came_from[current]
+
+                data.reverse()
+
+                return data
+
+        close_set.add(current)
+
+        for i, j in neighbors:
+
+            neighbor = current[0] + i, current[1] + j
+
+            tentative_g_score = gscore[current] + heuristic(current, neighbor)
+
+            if 0 <= neighbor[0] < array.shape[1]:
+
+                if 0 <= neighbor[1] < array.shape[0]:
+
+                    if array[neighbor[1]][neighbor[0]] == 0:
+
+                        continue
+
+                else:
+
+                    # array bound y walls
+
+                    continue
+
+            else:
+
+                # array bound x walls
+
+                continue
+
+            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
+
+                continue
+
+            if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:
+
+                came_from[neighbor] = current
+
+                gscore[neighbor] = tentative_g_score
+
+                fscore[neighbor] = tentative_g_score + \
+                    heuristic(neighbor, goal)
+
+                heapq.heappush(oheap, (fscore[neighbor], neighbor))
+
+    return False

astarex.py

@@ -0,0 +1,208 @@
+import numpy as np
+
+import heapq
+
+import matplotlib.pyplot as plt
+
+from matplotlib.pyplot import figure
+
+
+##############################################################################
+
+# plot grid
+
+##############################################################################
+
+
+grid = np.array([
+
+    [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0],
+
+    [0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
+
+    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
+
+# start point and goal
+
+start = (0, 0)
+
+goal = (0, 19)
+
+
+##############################################################################
+
+# heuristic function for path scoring
+
+##############################################################################
+
+
+def heuristic(a, b):
+
+    return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)
+
+
+##############################################################################
+
+# path finding function
+
+##############################################################################
+
+
+def astar(array, start, goal):
+
+    neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0),
+                 (1, 1), (1, -1), (-1, 1), (-1, -1)]
+
+    close_set = set()
+
+    came_from = {}
+
+    gscore = {start: 0}
+
+    fscore = {start: heuristic(start, goal)}
+
+    oheap = []
+
+    heapq.heappush(oheap, (fscore[start], start))
+
+    while oheap:
+
+        current = heapq.heappop(oheap)[1]
+
+        if current == goal:
+
+            data = []
+
+            while current in came_from:
+
+                data.append(current)
+
+                current = came_from[current]
+
+            return data
+
+        close_set.add(current)
+
+        for i, j in neighbors:
+
+            neighbor = current[0] + i, current[1] + j
+
+            tentative_g_score = gscore[current] + heuristic(current, neighbor)
+
+            if 0 <= neighbor[0] < array.shape[0]:
+
+                if 0 <= neighbor[1] < array.shape[1]:
+
+                    if array[neighbor[0]][neighbor[1]] == 1:
+
+                        continue
+
+                else:
+
+                    # array bound y walls
+
+                    continue
+
+            else:
+
+                # array bound x walls
+
+                continue
+
+            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
+
+                continue
+
+            if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:
+
+                came_from[neighbor] = current
+
+                gscore[neighbor] = tentative_g_score
+
+                fscore[neighbor] = tentative_g_score + \
+                    heuristic(neighbor, goal)
+
+                heapq.heappush(oheap, (fscore[neighbor], neighbor))
+
+    return False
+
+
+route = astar(grid, start, goal)
+
+route = route + [start]
+
+route = route[::-1]
+
+print(route)
+
+
+##############################################################################
+
+# plot the path
+
+##############################################################################
+
+
+# extract x and y coordinates from route list
+
+x_coords = []
+
+y_coords = []
+
+for i in (range(0, len(route))):
+
+    x = route[i][0]
+
+    y = route[i][1]
+
+    x_coords.append(x)
+
+    y_coords.append(y)
+
+# plot map and path
+
+fig, ax = plt.subplots(figsize=(20, 20))
+
+ax.imshow(grid, cmap=plt.cm.Dark2)
+
+ax.scatter(start[1], start[0], marker="*", color="yellow", s=200)
+
+ax.scatter(goal[1], goal[0], marker="*", color="red", s=200)
+
+ax.plot(y_coords, x_coords, color="black")
+
+plt.show()

helpler.py

@@ -2,7 +2,7 @@ from config import MAP
 from models.Road import Road
 from models.Grass import Grass
 from models.House import House
-
+from config import MAP_HEIGHT, MAP_WIDTH
 from models.Trash import Trash
 
 
@@ -16,3 +16,7 @@ def Render_Element(x, y):
         return House(x, y)
     else:
         return Trash(x, y, item)
+
+
+def is_within_width(item): return item[0] >= 0 and item[0] < MAP_WIDTH
+def is_within_height(item): return item[1] >= 0 and item[1] < MAP_HEIGHT

main.py

@@ -41,7 +41,7 @@ _, draw_items, gc = env.reset()
 
 # Initialize A*
 
-__a_star__ = A_Star(draw_items, gc)
+__a_star__ = A_Star(draw_items, gc, env, refresh_screen)
 __a_star__.houses_with_trash()
 
 for item in draw_items:
@@ -57,7 +57,7 @@ clock = pygame.time.Clock()
 # know = Knowledge(draw_items, gc)
 
 # Game Loop
-run_ql = False
+run_a = False
 running = True
 while running:
     for event in pygame.event.get():
@@ -76,11 +76,43 @@ while running:
             if event.key == pygame.K_SPACE:
                 env.step(4)
                 env.step(5)
-                __a_star__.best_move()
                 # know.update()
                 # know.show()
+            if event.key == pygame.K_a:
+                __a_star__.get_to_dest()
+                run_a = True
+
             gc.render()
 
             refresh_screen()
+    if run_a:
+        houses, trashes = __a_star__.houses_with_trash()
+        if len(houses) == 0 and gc.mixed == 0 and gc.paper == 0 and gc.glass == 0 and gc.plastic == 0:
+            run_a = False
+
+        else:
+            route = __a_star__.get_to_dest()
+            if len(route) > 0:
+                x, y = route[0]
+                if x - gc.col != 0:
+                    if x - gc.col < 0:
+                        env.step(2)
+                    else:
+                        env.step(3)
+                elif y - gc.row != 0:
+                    if y - gc.row < 0:
+                        env.step(0)
+                    else:
+                        env.step(1)
+
+                time.sleep(0.3)
+
+            elif len(route) == 0:
+                env.step(4)
+                env.step(5)
+
+        gc.render()
+
+        refresh_screen()
 
     clock.tick(30)

models/Garbage_Collector.py

@@ -125,7 +125,7 @@ class Garbage_Collector(Numbers):
         houses_around = False
         transfered = 0
         for field in to_check:
-            if field["row"] >= 0 and field["row"] < MAP_HEIGHT and field["col"] >= 0 and field["col"] < MAP_WIDTH:
+            if 0 <= field["row"] < MAP_HEIGHT and 0 <= field["col"] < MAP_WIDTH:
                 item = self.draw_items[(field["col"], field["row"])]
                 if isinstance(item, House):
                     houses_around = True
@@ -172,7 +172,7 @@ class Garbage_Collector(Numbers):
         transfered = 0
         trashes_around = False
         for field in to_check:
-            if field["row"] >= 0 and field["row"] < MAP_HEIGHT and field["col"] >= 0 and field["col"] < MAP_WIDTH:
+            if 0 <= field["row"] < MAP_HEIGHT and 0 <= field["col"] < MAP_WIDTH:
                 item = self.draw_items[(field["col"], field["row"])]
                 if isinstance(item, Trash):
                     trashes_around = True