Merge pull request 'movement_planning' (#2) from movement_planning into master

Reviewed-on: #2

astar.py

@@ -0,0 +1,61 @@
+from succ import succ as successors
+from queue import PriorityQueue
+from state import State
+
+def astar(istate, goalx, goaly, passedFields):
+    fringe = PriorityQueue()
+    fringe.put(istate)
+    explored = set()
+    steps = []
+    while not fringe.empty():
+        state = fringe.get()
+        if state.xpos == goalx and state.ypos == goaly:
+            steps.insert(0, state)
+            while (state.parent != None):
+                state = state.parent
+                steps.insert(0, state)
+            return steps
+
+        element = successors(state, passedFields, goalx, goaly)
+        explored.add((state.xpos, state.ypos, state.orientation))
+        for value in element:
+            val = (value.xpos, value.ypos, value.orientation)
+            if val in explored:
+                continue 
+            cost = state.priority + value.priority
+            succesorState = State(state, value.action, 
+                                  value.xpos, value.ypos, 
+                                  value.orientation, cost, 
+                                  value.heuristic)
+            if value not in fringe.queue:
+                fringe.put(succesorState)
+            else :
+                for element in fringe.queue:
+                    if (element.xpos==value.xpos and element.ypos==value.ypos) and element > value:
+                        element.parent = state
+                        element.priority = value.priority
+    return False
+
+
+# def bfs(istate, goalx, goaly, passedFields):
+#     fringe = [istate]
+#     explored = []
+#     steps = []
+#     while fringe:
+#         state = fringe.pop(0)
+#         if state.xpos == goalx and state.ypos == goaly:
+#             steps.insert(0, state)
+#             while (state.parent != None):
+#                 state = state.parent
+#                 steps.insert(0, state)
+#             return steps
+
+#         element = successors(state, passedFields)
+#         explored.append((state.xpos, state.ypos, state.orientation))
+#         for value in element:
+#             val = (value.xpos, value.ypos, value.orientation)
+#             if val not in explored and value not in fringe:
+#                 fringe.append(value)
+#     return False
+
+

garbage_truck.py

@@ -1,3 +1,5 @@
+FIELDWIDTH = 50
+
 class GarbageTank:
     def __init__(self, volume_capacity, mass_capacity):
         self.vcapacity = volume_capacity #m^3
@@ -8,10 +10,27 @@ class Engine:
         self.power = power #HP
 
 class GarbageTruck:
-    def __init__(self, dump_location, fuel_capacity, start_pos):
+    def __init__(self, dump_location, fuel_capacity, rect, orientation):
         self.dump_location = dump_location
         self.tank = GarbageTank(15, 18000)
         self.engine = Engine(400)
         self.fuel = fuel_capacity
-        self.pos = start_pos
+        self.rect = rect
+        self.orientation = orientation
         self.houses = [] #lista domów do odwiedzenia
+
+    def turn_left(self):
+        self.orientation = (self.orientation - 1) % 4
+
+    def turn_right(self):
+        self.orientation = (self.orientation + 1) % 4
+
+    def forward(self):
+        if self.orientation == 0:
+            self.rect.x += FIELDWIDTH
+        elif self.orientation == 1:
+            self.rect.y += FIELDWIDTH
+        elif self.orientation == 2:
+            self.rect.x -= FIELDWIDTH
+        else:
+            self.rect.y -= FIELDWIDTH

heuristicfn.py

@@ -0,0 +1,3 @@
+def heuristicfn(startx, starty, goalx, goaly):
+    return abs(startx - goalx) + abs(starty - goaly)
+    # return pow(((startx//50)-(starty//50)),2) + pow(((goalx//50)-(goaly//50)),2)

main.py

@@ -1,5 +1,10 @@
 import pygame
 import random
+from astar import astar
+from state import State
+import time
+from garbage_truck import GarbageTruck
+from heuristicfn import heuristicfn
 
 pygame.init()
 WIDTH, HEIGHT = 800, 800
@@ -16,52 +21,90 @@ SAND = pygame.transform.scale(SAND_IMG, (50, 50))
 COBBLE_IMG = pygame.image.load("cobble.jpeg")
 COBBLE = pygame.transform.scale(COBBLE_IMG, (50, 50))
 FPS = 10
+FIELDCOUNT = 16
+FIELDWIDTH = 50
+
+
+class Agent:
+    def __init__(self, rect, direction):
+        self.rect = rect
+        self.direction = direction
 
 
 def randomize_map():        # tworzenie mapy z losowymi polami
-    fields_list = [DIRT, GRASS, SAND, COBBLE]
     field_array_1 = []
     field_array_2 = []
+    field_priority = []
     for i in range(16):
+        temp_priority = []
         for j in range(16):
-            field_array_2.append(random.choice(fields_list))
+            if i in (0, 1) and j in (0, 1):
+                field_array_2.append(GRASS)
+                temp_priority.append(1)
+            else:
+                prob = random.uniform(0, 100)
+                if 0 <= prob <= 12:
+                    field_array_2.append(COBBLE)
+                    temp_priority.append(3)
+                elif 12 < prob <= 24:
+                    field_array_2.append(SAND)
+                    temp_priority.append(2)
+                else:
+                    field_array_2.append(GRASS)
+                    temp_priority.append(1)
         field_array_1.append(field_array_2)
         field_array_2 = []
-    return field_array_1
+        field_priority.append(temp_priority)
+    return field_array_1, field_priority
 
 
-def draw_window(agent, fields):
+def draw_window(agent, fields, flip):
+    if flip:
+        direction = pygame.transform.flip(AGENT, True, False)
+    else:
+        direction = pygame.transform.flip(AGENT, False, False)
     for i in range(16):
         for j in range(16):
             window.blit(fields[i][j], (i * 50, j * 50))
-    window.blit(AGENT, (agent.x, agent.y))      # wyswietlanie agenta
+    window.blit(direction, (agent.rect.x, agent.rect.y))  # wyswietlanie agenta
     pygame.display.update()
 
 
-def agent_movement(keys_pressed, agent):    # sterowanie
-    if keys_pressed[pygame.K_LEFT] and agent.x > 0:
-        agent.x -= 50
-    if keys_pressed[pygame.K_RIGHT] and agent.x < 750:
-        agent.x += 50
-    if keys_pressed[pygame.K_UP] and agent.y > 0:
-        agent.y -= 50
-    if keys_pressed[pygame.K_DOWN] and agent.y < 750:
-        agent.y += 50
-
-
 def main():
     clock = pygame.time.Clock()
     run = True
-    agent = pygame.Rect(0, 0, 50, 50)   # tworzenie pola dla agenta
-    fields = randomize_map()
+    x, y = [0, 0]
+    agent = GarbageTruck(0, 0, pygame.Rect(x, y, 50, 50), 0)   # tworzenie pola dla agenta
+    fields, priority_array = randomize_map()
+    print(priority_array)
+    final_x, final_y = [100, 300]
     while run:
         clock.tick(FPS)
-        for event in pygame.event.get():    # przechwycanie zamknięcia okna
+        for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 run = False
-        keys_pressed = pygame.key.get_pressed()
-        draw_window(agent, fields)
-        agent_movement(keys_pressed, agent)
+        # keys_pressed = pygame.key.get_pressed()
+        draw_window(agent, fields, False)   # false = kierunek east (domyslny), true = west
+        steps = astar(State(None, None, x, y, 'E', priority_array[0][0], heuristicfn(x, y, final_x, final_y)), final_x, final_y, priority_array)
+        for interm in steps:
+            if interm.action == 'LEFT':
+                agent.turn_left()
+                draw_window(agent, fields, True)
+            elif interm.action == 'RIGHT':
+                agent.turn_right()
+                draw_window(agent, fields, False)
+            elif interm.action == 'FORWARD':
+                agent.forward()
+                if agent.orientation == 0:
+                    draw_window(agent, fields, False)
+                elif agent.orientation == 2:
+                    draw_window(agent, fields, True)
+                else:
+                    draw_window(agent, fields, False)
+            time.sleep(0.5)
+    
+        while True:
+            pass  
 
     pygame.quit()
 

state.py

@@ -0,0 +1,11 @@
+class State:
+    def __init__(self, parent, action, xpos, ypos, orientation, priority, heuristic):
+        self.parent = parent
+        self.xpos = xpos
+        self.ypos = ypos
+        self.orientation = orientation
+        self.action = action
+        self.priority = priority
+        self.heuristic = heuristic
+    def __gt__(self, other):
+        return self.priority > other.priority 

succ.py

@@ -0,0 +1,31 @@
+from state import State
+from heuristicfn import heuristicfn
+FIELDWIDTH, FIELDCOUNT = 50, 16
+
+def succ(st: State, passedPriorities, goalx, goaly):
+    successors = []
+
+    if st.orientation == 'N':
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        if st.ypos > 0:
+            successors.append(State(st, 'FORWARD', st.xpos, st.ypos - FIELDWIDTH , 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+    
+    if st.orientation == 'S':
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        if st.ypos < FIELDWIDTH * (FIELDCOUNT  - 1):
+            successors.append(State(st, 'FORWARD', st.xpos, st.ypos + FIELDWIDTH , 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+
+    if st.orientation == 'W':
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        if st.xpos > 0:
+            successors.append(State(st, 'FORWARD', st.xpos - FIELDWIDTH , st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+
+    if st.orientation == 'E':
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        if st.xpos < FIELDWIDTH * (FIELDCOUNT  - 1):
+            successors.append(State(st, 'FORWARD', st.xpos + FIELDWIDTH , st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+    return successors