Merge branch 'master' into moving-fixes
This commit is contained in:
marcinljablonski
commit
fbde8d4abc
246
tractor.py
246
tractor.py
@ -4,137 +4,145 @@ 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
|
||||
|
||||
class Tractor:
|
||||
sleep_time = 1
|
||||
graph = Graph()
|
||||
last_location = None
|
||||
|
||||
while not frontier.empty():
|
||||
for key in graph.nodes:
|
||||
print ("key: %s , value: %s" % (key, graph.nodes[key]))
|
||||
def __init__(self, current_location, current_rotation):
|
||||
self.current_location = current_location
|
||||
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:
|
||||
for direction in directions:
|
||||
if direction == last_node:
|
||||
continue
|
||||
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()
|
||||
|
||||
await rotate(direction)
|
||||
result = await try_move()
|
||||
if result == "OK\n":
|
||||
graph.add_neighbor(current, direction)
|
||||
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
|
||||
|
||||
current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
|
||||
last_node = directions[(directions.index(current_rotation) - 1) % 4]
|
||||
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()
|
||||
|
||||
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
|
||||
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 current == goal:
|
||||
break
|
||||
if final_direction != direction:
|
||||
await self.rotate(final_direction)
|
||||
|
||||
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)
|
||||
print("idziemy na " + final_direction)
|
||||
|
||||
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())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user