refactored to object-oriented form
This commit is contained in:
marcinljablonski
parent
6cfeedcd7e
commit
c3260bf726
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]
|
||||
|
||||
|
||||
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(" ")
|
||||
|
||||
214
tractor.py
214
tractor.py
@ -2,110 +2,148 @@ import asyncio
|
||||
import sys
|
||||
import time
|
||||
from queue import PriorityQueue
|
||||
from Graph import Graph
|
||||
|
||||
def heuristic(a, b):
|
||||
(x1, y1) = a
|
||||
(x2, y2) = b
|
||||
return abs(x1 - x2) + abs(y1 - y2)
|
||||
class Tractor:
|
||||
sleep_time = 1
|
||||
graph = Graph()
|
||||
last_location = None
|
||||
|
||||
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 __init__(self, current_location, current_rotation):
|
||||
self.current_location = current_location
|
||||
self.current_rotation = 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
|
||||
|
||||
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
|
||||
|
||||
|
||||
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 move_tractor(self, start_position, goal):
|
||||
directions = ['N', 'W', 'S', 'E']
|
||||
self.current_rotation = 'N'
|
||||
last_node = 'N'
|
||||
last_location = start_position
|
||||
|
||||
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)
|
||||
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(0.7)
|
||||
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(0.7)
|
||||
writer.close()
|
||||
frontier = PriorityQueue()
|
||||
frontier.put(start_position, 0)
|
||||
cost_so_far = {}
|
||||
cost_so_far[start_position] = 0
|
||||
start_flag = True
|
||||
|
||||
|
||||
async def move_tractor(start_position, goal):
|
||||
directions = ['N', 'W', 'S', 'E']
|
||||
current_rotation = 'N'
|
||||
last_node = None
|
||||
current_position = start_position
|
||||
while not frontier.empty():
|
||||
# for key in self.graph.nodes:
|
||||
# print ("key: %s , value: %s" % (key, self.graph.nodes[key]))
|
||||
|
||||
frontier = PriorityQueue()
|
||||
frontier.put(start, 0)
|
||||
cost_so_far = {}
|
||||
cost_so_far[start] = 0
|
||||
start_flag = True
|
||||
self.current_location = frontier.get()
|
||||
|
||||
while not frontier.empty():
|
||||
local_graph = []
|
||||
if start_flag:
|
||||
for direction in directions:
|
||||
if direction == last_node:
|
||||
continue
|
||||
|
||||
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)
|
||||
|
||||
await rotate(direction)
|
||||
self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
|
||||
last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
|
||||
|
||||
result = await try_move()
|
||||
self.current_rotation = await self.move_to_current_location(last_location)
|
||||
# print("Jestem na %s , %s" % (self.current_location[0], self.current_location[1]))
|
||||
last_location = self.current_location
|
||||
|
||||
if result == "OK\n":
|
||||
local_graph.append(addnode(current_position, direction))
|
||||
if self.current_location == goal:
|
||||
break
|
||||
|
||||
current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
|
||||
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)
|
||||
|
||||
|
||||
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 current == goal:
|
||||
break
|
||||
|
||||
for next in local_graph:
|
||||
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
|
||||
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())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user