Merge branch 'master' into moving-fixes

tractor.py

@@ -4,137 +4,145 @@ import time
 from queue import PriorityQueue
 from Graph import Graph
 
-sleep_time = 1
+class Tractor:
-
+    sleep_time = 1
-def heuristic(a, b):
-    (x1, y1) = a
-    (x2, y2) = b
-    return abs(x1 - x2) + abs(y1 - y2)
-
-async def rotate(direction):
-    reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
-    writer.write(("rotate " + direction + "\n").encode())
-    await reader.readline()
-    time.sleep(sleep_time)
-    writer.close()
-
-async def try_move():
-    reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
-    writer.write("try\n".encode())
-    data = await reader.readline()
-    result = data.decode()
-    time.sleep(sleep_time)
-    writer.close()
-    return result
-
-async def move():
-    reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
-    writer.write("move\n".encode())
-    await reader.readline()
-    time.sleep(sleep_time)
-    writer.close()
-
-async def move_to_neighbor(current, neighbor, direction):
-    x = current[0] - neighbor[0]
-    y = current[1] - neighbor[1]
-    if x > 0:
-        final_direction = 'E'
-    elif x < 0:
-        final_direction = 'w'
-    elif y > 0:
-        final_direction = 'S'
-    else:
-        final_direction = 'N'
-
-    if final_direction != direction:
-        await rotate(final_direction)
-
-    await move()
-    return final_direction
-
-async def move_to_current_location(graph, last_location, current_location, direction):
-    if last_location == current_location:
-        return direction
-
-    frontier = PriorityQueue()
-    frontier.put(last_location, 0)
-    came_from = {}
-    cost_so_far = {}
-    came_from = []
-    cost_so_far[last_location] = 0
-    final_direction = None
-
-    while not frontier.empty():
-        current = frontier.get()
-        if current == goal:
-            break
-        for next in graph.neighbors(current):
-            new_cost = cost_so_far[current] + 1
-            if next not in cost_so_far or new_cost < cost_so_far[next]:
-                cost_so_far[next] = new_cost
-                priority = new_cost + heuristic(goal, next)
-                frontier.put(next, priority)
-                came_from.append(current)
-
-    # print(came_from)
-    for node in came_from[1:]:
-        final_direction = await move_to_neighbor(current, node, final_direction)
-        current = node
-
-    return final_direction
-
-
-async def move_tractor(start_position, goal):
-    directions = ['N', 'W', 'S', 'E']
-    current_rotation = 'N'
-    last_node = 'N'
-    last_location = start_position
-
-    frontier = PriorityQueue()
-    frontier.put(start_position, 0)
-    cost_so_far = {}
-    cost_so_far[start_position] = 0
-    start_flag = True
-
     graph = Graph()
+    last_location = None
 
-    while not frontier.empty():
+    def __init__(self, current_location, current_rotation):
-        for key in graph.nodes:
+        self.current_location = current_location
-            print ("key: %s , value: %s" % (key, graph.nodes[key]))
+        self.current_rotation = current_rotation
 
-        current = frontier.get()
+    def heuristic(self, a, b):
+        (x1, y1) = a
+        (x2, y2) = b
+        return abs(x1 - x2) + abs(y1 - y2)
 
-        if start_flag:
+    async def rotate(self, direction):
-            for direction in directions:
+        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
-                if direction == last_node:
+        writer.write(("rotate " + direction + "\n").encode())
-                    continue
+        await reader.readline()
+        time.sleep(self.sleep_time)
+        writer.close()
 
-                await rotate(direction)
+    async def try_move(self):
-                result = await try_move()
+        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
-                if result == "OK\n":
+        writer.write("try\n".encode())
-                    graph.add_neighbor(current, direction)
+        data = await reader.readline()
+        result = data.decode()
+        time.sleep(self.sleep_time)
+        writer.close()
+        return result
 
-        current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
+    async def move(self):
-        last_node = directions[(directions.index(current_rotation) - 1) % 4]
+        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
+        writer.write("move\n".encode())
+        await reader.readline()
+        time.sleep(self.sleep_time)
+        writer.close()
 
-        current_rotation = await move_to_current_location(graph, last_location, current, current_rotation)
+    async def move_to_neighbor(self, current, neighbor, direction):
-        print("Jestem na %s , %s" % (current[0], current[1]))
+        x = current[0] - neighbor[0]
-        last_location = current
+        y = current[1] - neighbor[1]
+        if x > 0:
+            final_direction = 'N'
+        elif x < 0:
+            final_direction = 'S'
+        elif y > 0:
+            final_direction = 'W'
+        else:
+            final_direction = 'E'
 
-        if current == goal:
+        if final_direction != direction:
-            break
+            await self.rotate(final_direction)
 
-        for next in graph.neighbors(current):
+        print("idziemy na " + final_direction)
-            new_cost = cost_so_far[current] + 1
-            if next not in cost_so_far or new_cost < cost_so_far[next]:
-                cost_so_far[next] = new_cost
-                priority = new_cost + heuristic(goal, next)
-                frontier.put(next, priority)
 
-        start_flag = False
+        await self.move()
+        return final_direction
+
+    async def move_to_current_location(self, last_location):
+        if last_location == self.current_location:
+            return self.current_rotation
+
+        # frontier = PriorityQueue()
+        # frontier.put(last_location, 0)
+        # came_from = {}
+        # cost_so_far = {}
+        # came_from = []
+        # cost_so_far[last_location] = 0
+        # final_direction = self.current_rotation
+
+        # while not frontier.empty():
+        #     current = frontier.get()
+        #     if current == goal:
+        #         break
+        #     for next in self.graph.neighbors(current):
+        #         new_cost = cost_so_far[current] + 1
+        #         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)
+        #             came_from.append(current)
+
+        # for node in came_from[1:]:
+        #     final_direction = await self.move_to_neighbor(current, node, final_direction)
+        #     current = node
+        final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
+
+        return final_direction
+
+
+    async def move_tractor(self, start_position, goal):
+        directions = ['N', 'W', 'S', 'E']
+        self.current_rotation = 'N'
+        last_node = 'N'
+        last_location = start_position
+
+        frontier = PriorityQueue()
+        frontier.put(start_position, 0)
+        cost_so_far = {}
+        cost_so_far[start_position] = 0
+        start_flag = True
+
+
+        while not frontier.empty():
+            # for key in self.graph.nodes:
+                # print ("key: %s , value: %s" % (key, self.graph.nodes[key]))
+
+            self.current_location = frontier.get()
+
+            if start_flag:
+                for direction in directions:
+                    if direction == last_node:
+                        continue
+
+                    await self.rotate(direction)
+                    result = await self.try_move()
+                    if result == "OK\n":
+                        self.graph.add_neighbor(self.current_location, direction)
+
+            self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
+            last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
+
+            self.current_rotation = await self.move_to_current_location(last_location)
+            last_location = self.current_location
+
+            if self.current_location == goal:
+                break
+
+            for next in self.graph.neighbors(self.current_location):
+                new_cost = cost_so_far[self.current_location] + 1
+                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)
+
+            start_flag = False
 
 if __name__ == "__main__":
     start = (0,0)
     goal = (10,6)
-    asyncio.run(move_tractor(start, goal))
+    tractor = Tractor((0,0), 'N')
+    asyncio.run(tractor.move_tractor(start, goal))
     # asyncio.run(test())