fixed moving to specific position and added dfs walk

Graph.py

@@ -3,7 +3,7 @@ class Graph:
         self.nodes = {}
     
     def neighbors(self, id):
-        return self.nodes[id]
+        return self.nodes[id][0]
 
     def direction2point(self, point, direction):
         if direction == 'N':
@@ -18,11 +18,11 @@ class Graph:
     def add_neighbor(self, node, direction):
         neighbor = self.direction2point(node, direction)
         if node in self.nodes:
-            self.nodes[node].append(neighbor)
+            self.nodes[node][0].add(neighbor)
         else:
-            self.nodes[node] = [neighbor]
+            self.nodes[node] = (set([neighbor]), False)
         if neighbor in self.nodes:
-            self.nodes[neighbor].append(node)
+            self.nodes[neighbor][0].add(node)
         else:
-            self.nodes[neighbor] = [node]
+            self.nodes[neighbor] = (set([node]), False)
 

README.md

@@ -0,0 +1 @@
+cała logika ktora oglada roslinke a potem ja np podlewa powinna byc wywolywana w metodzie service w tractor.py

env.py

@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import string
 import sys
 import os

tractor.py

@@ -1,13 +1,11 @@
+#!/usr/bin/env python3
 import asyncio
-import sys
 import time
-from queue import PriorityQueue
 from Graph import Graph
 
 class Tractor:
-    sleep_time = 1
+    sleep_time = 0.1
     graph = Graph()
-    last_location = None
     moved_flag = False
     directions = ['N', 'W', 'S', 'E']
 
@@ -59,33 +57,39 @@ class Tractor:
         if final_direction != direction:
             await self.rotate(final_direction)
 
-        print("idziemy na " + final_direction)
 
         await self.move()
         return final_direction
 
-    def dfs_paths(self, start, goal):
-        stack = [(start, [start])]
-        visited = []
-        while stack:
-            (vertex, path) = stack.pop()
-            if vertex not in visited:
-                if vertex == goal:
-                    return path
-                visited.append(vertex)
-                for neighbor in self.graph.nodes[vertex]:
-                    stack.append((neighbor, path + [neighbor]))
+    def bfs_shortest_path(self, start, goal):
+        explored = []
+        queue = [[start]]
+
+        if start == goal:
+            return []
+
+        while queue:
+            path = queue.pop(0)
+            node = path[-1]
+            if node not in explored:
+                neighbours = self.graph.nodes[node][0]
+                for neighbour in neighbours:
+                    new_path = list(path)
+                    new_path.append(neighbour)
+                    queue.append(new_path)
+                    if neighbour == goal:
+                        return new_path
+
+                explored.append(node)
 
 
     async def move_to_current_location(self, last_location):
         if last_location == self.current_location:
             return self.current_rotation
 
-        path = self.dfs_paths(last_location, self.current_location)
+        path = self.bfs_shortest_path(last_location, self.current_location)
         path = path[1:]
 
-        print("~~~~~~~~~~~~~~~~~~")
-        print(path)
         pos = last_location
         final_direction = self.current_rotation
 
@@ -93,50 +97,62 @@ class Tractor:
             final_direction = await self.move_to_neighbor(pos, node, final_direction)
             pos = node
 
-        # # final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
         self.moved_flag = True
 
         return final_direction
 
     async def look_around(self):
-        for direction in self.directions:
-            if direction == self.last_node:
+        for i in range(4):
+            if i == 2:
                 continue
-
+            direction = self.directions[(self.directions.index(self.current_rotation) + i) % 4]
             await self.rotate(direction)
             result = await self.try_move()
             if result == "OK\n":
                 self.graph.add_neighbor(self.current_location, direction)
 
+        self.current_rotation = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
 
-    async def move_tractor(self, start_position, goal):
-        self.current_rotation = 'N'
+    def find_optimal(self, frontier, last_location):
+        index = 0
+        cost = frontier[0][1] + len(self.bfs_shortest_path(last_location, frontier[0][0]))
+        for i, n in enumerate(frontier):
+            tmp_cost = n[1] + len(self.bfs_shortest_path(last_location, n[0]))
+            if tmp_cost < cost:
+                cost = tmp_cost
+                index = i
+        return index
+
+    def mark_as_visited(self):
+        tmp_set = self.graph.nodes[self.current_location][0]
+        self.graph.nodes[self.current_location] = (tmp_set, True)
+
+    async def move_tractor(self, goal):
+        start_position = self.current_location
         last_location = start_position
 
-        frontier = PriorityQueue()
-        frontier.put(start_position, 0)
+        frontier = []
+        frontier.append((start_position, 0))
         cost_so_far = {}
         cost_so_far[start_position] = 0
 
         await self.look_around()
+        self.mark_as_visited()
 
-        while not frontier.empty():
-            # for key in self.graph.nodes:
-                # print ("key: %s , value: %s" % (key, self.graph.nodes[key]))
+        while frontier:
 
-            self.current_location = frontier.get()
-
-            if self.moved_flag:
-                await self.look_around()
-
-            self.current_rotation = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
-            self.last_node = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
+            optimal_index = self.find_optimal(frontier, last_location)
+            self.current_location = frontier.pop(optimal_index)[0]
 
             self.current_rotation = await self.move_to_current_location(last_location)
-            # print(last_location)
-            # print(self.current_location)
             last_location = self.current_location
 
+            if self.moved_flag and not self.graph.nodes[self.current_location][1]:
+                await self.look_around()
+                self.mark_as_visited()
+
+            self.last_node = self.directions[(self.directions.index(self.current_rotation) - 2) % 4]
+
             if self.current_location == goal:
                 break
 
@@ -145,12 +161,32 @@ class Tractor:
                 if next not in cost_so_far or new_cost < cost_so_far[next]:
                     cost_so_far[next] = new_cost
                     priority = new_cost + self.heuristic(goal, next)
-                    frontier.put(next, priority)
+                    frontier.append((next, priority))
+
+    async def run(self):
+        start_position = self.current_location
+        # await self.move_tractor(start_position)
+        while True:
+            visited, stack = set(), [start_position]
+            while stack:
+                vertex = stack.pop()
+                if vertex not in visited:
+                    visited.add(vertex)
+                    await self.move_tractor(vertex)
+                    self.current_location = vertex
+                    stack.extend(self.graph.nodes[vertex][0] - visited)
+                    await self.service()
+
+            await self.move_tractor(start_position)
+
+    async def service(self):
+        "tutaj trzeba zapytac env z jakimi roslinami sie styka traktor i je obsłuzyc"
+        pass
 
 
 if __name__ == "__main__":
     start = (0,0)
     goal = (10,6)
     tractor = Tractor((0,0), 'N')
-    asyncio.run(tractor.move_tractor(start, goal))
-    # asyncio.run(test())
+    # asyncio.run(tractor.move_tractor(start, goal))
+    asyncio.run(tractor.run())