157 lines
4.9 KiB
Python
157 lines
4.9 KiB
Python
import asyncio
|
|
import sys
|
|
import time
|
|
from queue import PriorityQueue
|
|
from Graph import Graph
|
|
|
|
class Tractor:
|
|
sleep_time = 1
|
|
graph = Graph()
|
|
last_location = None
|
|
moved_flag = False
|
|
directions = ['N', 'W', 'S', 'E']
|
|
|
|
def __init__(self, current_location, current_rotation):
|
|
self.current_location = current_location
|
|
self.current_rotation = current_rotation
|
|
self.last_node = current_rotation
|
|
|
|
def heuristic(self, a, b):
|
|
(x1, y1) = a
|
|
(x2, y2) = b
|
|
return abs(x1 - x2) + abs(y1 - y2)
|
|
|
|
async def rotate(self, direction):
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write(("rotate " + direction + "\n").encode())
|
|
await reader.readline()
|
|
time.sleep(self.sleep_time)
|
|
writer.close()
|
|
|
|
async def try_move(self):
|
|
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(self.sleep_time)
|
|
writer.close()
|
|
return result
|
|
|
|
async def move(self):
|
|
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()
|
|
|
|
async def move_to_neighbor(self, current, neighbor, direction):
|
|
x = current[0] - neighbor[0]
|
|
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 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]))
|
|
|
|
|
|
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 = path[1:]
|
|
|
|
print("~~~~~~~~~~~~~~~~~~")
|
|
print(path)
|
|
pos = last_location
|
|
final_direction = self.current_rotation
|
|
|
|
for node in path:
|
|
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:
|
|
continue
|
|
|
|
await self.rotate(direction)
|
|
result = await self.try_move()
|
|
if result == "OK\n":
|
|
self.graph.add_neighbor(self.current_location, direction)
|
|
|
|
|
|
async def move_tractor(self, start_position, goal):
|
|
self.current_rotation = 'N'
|
|
last_location = start_position
|
|
|
|
frontier = PriorityQueue()
|
|
frontier.put(start_position, 0)
|
|
cost_so_far = {}
|
|
cost_so_far[start_position] = 0
|
|
|
|
await self.look_around()
|
|
|
|
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 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]
|
|
|
|
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.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)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
start = (0,0)
|
|
goal = (10,6)
|
|
tractor = Tractor((0,0), 'N')
|
|
asyncio.run(tractor.move_tractor(start, goal))
|
|
# asyncio.run(test())
|