141 lines
3.9 KiB
Python
141 lines
3.9 KiB
Python
import asyncio
|
|
import sys
|
|
import time
|
|
from queue import PriorityQueue
|
|
from Graph import Graph
|
|
|
|
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()
|
|
|
|
while not frontier.empty():
|
|
for key in graph.nodes:
|
|
print ("key: %s , value: %s" % (key, graph.nodes[key]))
|
|
|
|
current = frontier.get()
|
|
|
|
if start_flag:
|
|
for direction in directions:
|
|
if direction == last_node:
|
|
continue
|
|
|
|
await rotate(direction)
|
|
result = await try_move()
|
|
if result == "OK\n":
|
|
graph.add_neighbor(current, direction)
|
|
|
|
current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
|
|
last_node = directions[(directions.index(current_rotation) - 1) % 4]
|
|
|
|
current_rotation = await move_to_current_location(graph, last_location, current, current_rotation)
|
|
print("Jestem na %s , %s" % (current[0], current[1]))
|
|
last_location = current
|
|
|
|
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)
|
|
|
|
start_flag = False
|
|
|
|
if __name__ == "__main__":
|
|
start = (0,0)
|
|
goal = (10,6)
|
|
asyncio.run(move_tractor(start, goal))
|
|
# asyncio.run(test())
|