.
This commit is contained in:
parent
6cfeedcd7e
commit
c75ecbb065
27
Graph.py
27
Graph.py
@ -1,7 +1,28 @@
|
||||
class SimpleGraph:
|
||||
class Graph:
|
||||
def __init__(self):
|
||||
self.edges = {}
|
||||
self.nodes = {}
|
||||
|
||||
def neighbors(self, id):
|
||||
return self.edges[id]
|
||||
return self.nodes[id]
|
||||
|
||||
def direction2point(self, point, direction):
|
||||
if direction == 'N':
|
||||
return (point[0] - 1, point[1])
|
||||
elif direction == 'S':
|
||||
return (point[0] + 1, point[1])
|
||||
elif direction == 'W':
|
||||
return (point[0], point[1] - 1)
|
||||
else:
|
||||
return (point[0], point[1] + 1)
|
||||
|
||||
def add_neighbor(self, node, direction):
|
||||
neighbor = self.direction2point(node, direction)
|
||||
if node in self.nodes:
|
||||
self.nodes[node].append(neighbor)
|
||||
else:
|
||||
self.nodes[node] = [neighbor]
|
||||
if neighbor in self.nodes:
|
||||
self.nodes[neighbor].append(node)
|
||||
else:
|
||||
self.nodes[neighbor] = [node]
|
||||
|
||||
|
17
Point.py
17
Point.py
@ -1,17 +0,0 @@
|
||||
class Point:
|
||||
def __init__(self, cord):
|
||||
self.x = cord[0]
|
||||
self.y = cord[1]
|
||||
|
||||
def get_cord(self):
|
||||
return (self.x, self.y)
|
||||
|
||||
def get_x(self):
|
||||
return self.x
|
||||
|
||||
def get_y(self):
|
||||
return self.y
|
||||
|
||||
def set_cord(self, cord):
|
||||
self.x = cord[0]
|
||||
self.y = cord[1]
|
1
env.py
1
env.py
@ -37,6 +37,7 @@ def print_field(field, tractor):
|
||||
os.system("cls")
|
||||
else:
|
||||
os.system("clear")
|
||||
print(tractor.get_position())
|
||||
sys.stdout.write(OKBLUE)
|
||||
sys.stdout.write("$")
|
||||
sys.stdout.write(" ")
|
||||
|
135
tractor.py
135
tractor.py
@ -2,40 +2,20 @@ 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)
|
||||
|
||||
def addnode(current, direction):
|
||||
if direction == 'N':
|
||||
return (current[0] - 1, current[1])
|
||||
elif direction == 'S':
|
||||
return (current[0] + 1, current[1])
|
||||
elif direction == 'W':
|
||||
return (current[0], current[1] - 1)
|
||||
else:
|
||||
return (current[0], current[1] + 1)
|
||||
|
||||
|
||||
def getrotation(a, b):
|
||||
y = b[0] - a[0]
|
||||
x = b[1] - a[1]
|
||||
if y > 0:
|
||||
return 'S'
|
||||
elif y < 0:
|
||||
return 'N'
|
||||
elif x > 0:
|
||||
return 'E'
|
||||
else:
|
||||
return 'W'
|
||||
|
||||
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(0.7)
|
||||
time.sleep(sleep_time)
|
||||
writer.close()
|
||||
|
||||
async def try_move():
|
||||
@ -43,7 +23,7 @@ async def try_move():
|
||||
writer.write("try\n".encode())
|
||||
data = await reader.readline()
|
||||
result = data.decode()
|
||||
time.sleep(0.7)
|
||||
time.sleep(sleep_time)
|
||||
writer.close()
|
||||
return result
|
||||
|
||||
@ -51,52 +31,100 @@ async def move():
|
||||
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
||||
writer.write("move\n".encode())
|
||||
await reader.readline()
|
||||
time.sleep(0.7)
|
||||
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 = None
|
||||
current_position = start_position
|
||||
last_node = 'N'
|
||||
last_location = start_position
|
||||
|
||||
frontier = PriorityQueue()
|
||||
frontier.put(start, 0)
|
||||
frontier.put(start_position, 0)
|
||||
cost_so_far = {}
|
||||
cost_so_far[start] = 0
|
||||
cost_so_far[start_position] = 0
|
||||
start_flag = True
|
||||
|
||||
graph = Graph()
|
||||
|
||||
while not frontier.empty():
|
||||
local_graph = []
|
||||
|
||||
for direction in directions:
|
||||
if direction == last_node:
|
||||
continue
|
||||
|
||||
await rotate(direction)
|
||||
|
||||
result = await try_move()
|
||||
|
||||
if result == "OK\n":
|
||||
local_graph.append(addnode(current_position, direction))
|
||||
|
||||
current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
|
||||
|
||||
for key in graph.nodes:
|
||||
print ("key: %s , value: %s" % (key, graph.nodes[key]))
|
||||
|
||||
current = frontier.get()
|
||||
if not start_flag:
|
||||
rotation = getrotation(current_position, current)
|
||||
if rotation != current_rotation:
|
||||
await rotate(rotation)
|
||||
last_node = directions[(directions.index(rotation) - 2) % 4]
|
||||
await move()
|
||||
current_position = current
|
||||
|
||||
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 local_graph:
|
||||
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
|
||||
@ -109,3 +137,4 @@ if __name__ == "__main__":
|
||||
start = (0,0)
|
||||
goal = (10,6)
|
||||
asyncio.run(move_tractor(start, goal))
|
||||
# asyncio.run(test())
|
||||
|
Loading…
Reference in New Issue
Block a user