def __init__(self): Astar.__init__(self) Bfs.__init__(self) pygame.init() self.display_width = 1020+2+200 self.display_height = 570+2+300-210 self.screen = pygame.display.set_mode((self.display_width,self.display_height)) self.clock = pygame.time.Clock() pygame.display.set_caption(u'Path Finding Visualizer') self.width = 34 self.height =22 self.walls = list() self.maze = [[]] self.white = (255,255,255) self.red = (255,0,0) self.less_red = (150,0,0) self.blue = (0,255,0) self.less_blue = (0,150,0) self.green = (0,0,255) self.less_green = (0,0,150) self.black = (0,0,0) self.grid_color = (0,150,255)
def initialize(self):
'''
Initializes, and runs the initial domain filtering loop.
'''
self.gac.initialize(self.domains, self.constraints)
self.domains = self.gac.domain_filtering_loop()
self.astar = Astar(self)
def publish_astar(self):
obj = Astar()
obj.astar_func(self.path_array)
for i in range(len(self.path_array)):
rospy.sleep(1)
msg = AStarPath()
msg.data = self.path_array[i]
self.astar_pub.publish(msg)
def initialize(self): ''' Initializes by running the first domain filtering loop. ''' self.gac.initialize(self.domains, self.constraints) self.domains = self.gac.domain_filtering_loop() self.astar = Astar(self)
def __init__(self, app, pos):
self.app = app
self.grid_pos = pos
self.pix_pos = self.get_pix_pos()
self.step = 0
self.index = 0
self.flag = True
self.astar = Astar()
self.bfs = 0
def generate_paths(self, algo="a-star"):
if algo == "a-star":
for robot in self.robots:
astar = Astar(robot.pose, robot.goal, self.obstacles)
robot.path = astar.generate_path()
elif algo == "rrt":
for robot in self.robots:
rrt = RRT(robot.pose, robot.goal, self.obstacles)
robot.path = rrt.generate_path()
else:
print "No such algo currently exists."
return
def getPathFromAstar(self):
self.gettingAstarPath = True
if self.astarPath == None or self.getNextAstarPath:
self.astarPath = Astar(self.head.pos(), self.food.pos(),
self.getBodyPositions(), self.grid)
self.getNextAstarPath = False
try:
self.toward(self.astarPath[self.astarPath.index(self.head.pos()) +
1])
except:
ValueError
return self.astarPath
def run(n, a, display):
# s = ( (1,2,4,7,5,3,0,8,6),(2,0) )
# t = ( (1,2,3,4,5,6,7,8,0),(2,2) )
for i in range(n * n):
if a[i] == 0:
idx = i
s = (tuple(a), (idx // n, idx % n))
a = []
for i in range(1, n * n):
a.append(i)
a.append(0)
t = (tuple(a), (n - 1, n - 1))
algo = Astar(n, s, t)
while True:
if not algo.found:
idx = algo.search()
drawBoard(n, idx[0], display)
if idx == t:
path = algo.createPath()
time.sleep(3)
for idx in path:
drawBoard(n, idx[0], display)
time.sleep(2)
for event in pygame.event.get():
if event.type == pygame.QUIT:
exit()
def initialize(self):
"""
Initializes, and runs the initial domain filtering loop.
"""
self.gac.initialize(self.domains, self.constraints)
self.domains = self.gac.domain_filtering_loop()
self.astar = Astar(self)
def test_astar1(self):
nodes1 = {
"A": [("B", 2), ("D", 3)],
"B": [("A", 2), ("D", 5), ("C", 2)],
"C": [("B", 2), ("F", 1)],
"D": [("A", 3), ("B", 5), ("E", 8), ("G", 7)],
"E": [("D", 8), ("G", 5)],
"F": [("C", 1), ("H", 3)],
"G": [("E", 5), ("D", 7), ("H", 2)],
"H": [("G", 2), ("F", 3)]
}
astar1 = Astar(nodes1, "A", "H")
result1 = astar1.search()
self.assertEqual(result1, ['A', 'B', 'C', 'F', 'H'])
def search_exe(self):
Astar()
#self.path_pub.publish(path)
MDP()
QL()
self.finish_pub.publish(True)
rospy.sleep(10)
rospy.signal_shutdown("Finish Simulation")
def analysis():
aFile = open("analysis.csv","w+")
aFile.write("algo, totLengthOfSolution, avgLenghtOfSolution, totNoSolution, avgNoSolution, totExec, avgExec, optimality")
file = open(inputPath,"r")
pNum = 0
for line in file:
algoAnalysis(Astar(puzzleNumber=pNum, input=line, heuristic="h1"))
aFile.close()
def openBoard(self):
'''
Opens the file system, allowing you to select a board from a text file. It then builds a board based on what is selected.
'''
filename = askopenfilename(parent=root)
self.board = Board(filename)
self.gui.clear()
self.gui.build(self.board)
gui.pack(side="top", fill="both", expand="false")
self.astar = Astar(self.board.startnode)
def draw_random_graph(n):
""" vytvoří náhdný graf o n nodech a přetvoří ho do datové struktury grafu pomocí slovníku
a nalezne v něm cestu"""
try:
p = 0.15 if n < 80 else 0.05
G = ER(n, p)
a = nx.convert.to_dict_of_lists(G)
dict_values = a.items()
new_a = {str(key): value for key, value in dict_values}
dict_values2 = new_a.items()
for key, value in dict_values2:
new_a[key] = [(str(v), random.randint(1, 2)) for v in value]
astar = Astar(new_a, str(random.randint(0, n)),
str(random.randint(0, n)))
#print(astar.search())
shortest_path = [int(i) for i in astar.search()]
print(shortest_path)
node_colors = [
"blue" if n in shortest_path else "red" for n in G.nodes()
]
pos = nx.spring_layout(G)
nx.draw_networkx(G, pos, node_color=node_colors)
for i in range(0, len(shortest_path)):
if i + 1 == len(shortest_path):
break
nx.draw_networkx_edges(G,
pos,
edgelist=[(shortest_path[i],
shortest_path[i + 1])],
edge_color="blue",
width=2)
except:
print("cesta neexistuje")
def main(args):
# Astar Module
if args.model == "astar":
model = Astar()
# Reinforcement Learning module
elif args.model == "rl":
model = Rl()
# Dummy test
elif args.model == "dummy":
model = Dummy()
# Create an environment
env = gym.make('voilier-v2').unwrapped
state = env.reset()
# TODO Get range from argument
for step in range(0, 200, 1):
action = model.step(state)
state, _, _, _ = env.step(action)
env.render()
def __init__(self, n_features, n_actions):
self.n_features = n_features
self.n_actions = n_actions
# env, simulate observations
env = open('env_hole.pkl', 'rb')
self.observe_env = pickle.load(env)
env.close()
env = open('env_hole_vol.pkl', 'rb')
self.observe_vol = pickle.load(env)
env.close()
# current position
self.pos_x = None
self.pos_y = 1.0
self.pos_z = None
# 8 action dim
self.action_labels = [
'0', '45', '90', '135', '180', '225', '270', '315'
]
self.actor = Actor(self.n_features, self.n_actions, lr=0.004)
self.actor.load_trained_model("save/multiple/hole/save100.ckpt")
# fixme, first define critic before load : will report bug for not found in checkpoint
self.critic = Critic(self.n_features,
self.n_actions,
lr=0.003,
gamma=0.95)
# fixme, use trained model to predict
self.bin_graph = tf.Graph()
with self.bin_graph.as_default():
self.bin_classfic = BinSupervisor(366, 2)
# fixme, avoid obstacles, env defined in A star
self.astar = Astar()
def test_astar2(self):
nodes2 = {
"A": [("B", 2), ("E", 2)],
"B": [("A", 2), ("C", 2)],
"C": [("B", 2), ("D", 2)],
"D": [("C", 2), ("G", 2)],
"E": [("A", 2), ("J", 3)],
"F": [("B", 2), ("K", 2), ("G", 2)],
"G": [("F", 2), ("D", 2), ("L", 4), ("H", 3)],
"H": [("G", 3), ("I", 2)],
"I": [("H", 2), ("M", 3)],
"J": [("E", 3), ("O", 4)],
"K": [("F", 2), ("O", 2)],
"L": [("G", 4), ("M", 1), ("P", 5)],
"M": [("I", 3), ("L", 1)],
"O": [("K", 2), ("J", 4)],
"P": [("L", 5)]
}
astar2 = Astar(nodes2, "A", "P")
result2 = astar2.search()
self.assertEqual(result2, ['A', 'B', 'C', 'D', 'G', 'L', 'P'])
def run(self):
while not self.exit:
self.screen.fill((255, 255, 255))
self.draw_grid(self.screen, self.grid, self.grid_size, self.margin)
algorithm = Astar(self.grid, self.start, self.end)
path = algorithm.astar()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
if path == 'failure':
print('failure')
else:
for path_node in path:
self.show_node(self.screen, (52, 158, 235), path_node.x,
path_node.y, self.margin)
pygame.display.flip()
self.clock.tick(self.ticks)
pygame.quit()
def main_1():
dim = 3
goal = Puzzle(
np.insert(np.arange(1, dim * dim), dim * dim - 1, 0).reshape(dim, dim))
time1 = []
time2 = []
space1 = []
space2 = []
for i in range(15):
while True:
start = Puzzle(
np.random.permutation(np.arange(dim * dim)).reshape(
(dim, dim)))
if start.solvable(): break
t_1 = time.time()
p1 = Astar('manhattan', start, goal)
h1 = p1.search()
t_2 = time.time()
t_f = t_2 - t_1
time1.append(t_f)
space1.append(np.mean([j for i, j in h1]))
t_11 = time.time()
p2 = Astar('hamming', start, goal)
h2 = p2.search()
t_22 = time.time()
t_ff = t_22 - t_11
time2.append(t_ff)
space2.append(np.mean([j for i, j in h2]))
a = plt.plot(time1, 'blue')
b = plt.plot(time2, 'red')
plt.legend(['Manhattan', 'Hamming'])
plt.xlabel('puzzle')
plt.ylabel('tiempo')
plt.title('Tiempo A*')
plt.show()
a = plt.plot(space1, 'blue')
b = plt.plot(space2, 'red')
plt.legend(['Manhattan', 'Hamming'])
plt.xlabel('puzzle')
plt.ylabel('espacio')
plt.title('Espacio A*')
plt.show()
print(time1)
print(space1)
print(time2)
print(space2)
def animate_astar(self): a,path =Astar.search(self,self.maze,1,self.start_node,self.end_node) for a in a: pygame.draw.rect(self.screen,self.less_red,(a.position[0]*30,a.position[1]*30,29,29)) pygame.display.update() self.clock.tick(60) draw = [] for i,j in enumerate(path): for k,l in enumerate(j): if l is not -1: draw.append((i,k)) for i in draw[::-1]: pygame.draw.rect(self.screen,self.red,(i[0]*30,i[1]*30,29,29)) self.clock.tick(200) pygame.display.update()
class Probleminstance:
"""
Class holding a state of the problem, with domains and constraints. Also used to represent a node in astar.
"""
def __init__(self, domains, constraint_list):
"""
Initializes information, and sets the constraint formula.
"""
# A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
# GAC INFO
self.constraints = Constraints(
"column[y_index] == row[x_index]", ["column", "row", "x_index", "y_index"], constraint_list
)
self.domains = domains
# init gac
self.gac = Gac_nonogram()
def initialize(self):
"""
Initializes, and runs the initial domain filtering loop.
"""
self.gac.initialize(self.domains, self.constraints)
self.domains = self.gac.domain_filtering_loop()
self.astar = Astar(self)
def solve(self):
"""
Iteratively ran to find new states.
"""
self.current = self.astar.solve("A*")
return [self.current, self.astar.prev_current]
def is_solution(self):
"""
Returns true if this state is a solution state, false if not.
"""
for domain in self.domains:
if len(self.domains[domain]) != 1:
return False
return True
def get_neighbours(self):
"""
Returns the neighbours of this state.
"""
minlen = float("inf")
current_domain = None
neighbours = list()
for domain in self.domains:
if len(self.domains[domain]) == 1:
continue
if (len(self.domains[domain])) < minlen:
minlen = len(self.domains[domain])
current_domain = domain
for variation in self.domains[current_domain]:
copy_domains = copy.deepcopy(self.domains)
copy_domains[current_domain] = [variation]
copy_domains = self.gac.rerun(copy_domains, current_domain, self.constraints)
pi = Probleminstance(copy_domains, self.constraints.involved)
neighbours.append(pi)
self.neighbours = neighbours
return neighbours
def get_arc_cost(self):
"""
Returns the cost of moving from this state. Always 1 in this problem.
"""
return 1
def get_h(self):
"""
Returns the h value, based on the amount of possible values for each row and column.
"""
h = 0
for domain in self.domains:
h += len(self.domains[domain])
self.h = h
return h
def is_illegal(self):
"""
Returns true if this is a legal state, false if not.
"""
for node in self.domains:
if self.domains[node] == []:
return True
for constraint_node in self.constraints.involved[node]:
legal = False
for x_domain in self.domains[node]:
for y_domain in self.domains[constraint_node]:
x_index = node[1]
y_index = constraint_node[1]
if self.constraints.expression(x_domain, y_domain, x_index, y_index):
legal = True
if legal == False:
return True
return False
def __lt__(self, other):
"""
Less than comparison, compares on f primarily, h secondarily. Used by astar.
"""
if self.f == other.f:
return self.h < other.h
return self.f < other.f
def __eq__(self, other):
"""
Equals operator, checks if the domains are equal.
"""
return self.domains == other.domains
def __str__(self):
"""
String representation of this state's domains.
"""
return str(self.domains)
if __name__ == "__main__":
Game = CreateGame()
game = Game.create_game(int(sys.argv[1]),int(sys.argv[2]))
#game = [['D'],['E','F','I','J'],['B','G'],['C','H'],['A']]
#game = [['F', 'C', 'B', 'G', 'I', 'J','K', 'L','M', 'N', 'O','P', 'Q', 'A'], ['D', 'H'], ['E']]
target_game = Game.create_target(int(sys.argv[1]),int(sys.argv[2]))
#target_game = Game.create_target(3,10)
start = Node(game)
target = Node(target_game)
#heurstics = {"default":"Default (blocks out of place)","M":"Matching Rows","De":"Euclidean Distance","Dm":"Manhattan Distance"}
#heurstics = {"M":"matching rows"}
#heurstics = {"default":"default heur"}
heurstics = {"default":"default heur(out of place blocks)","M":"Matching rows"}
for h in heurstics.keys():
mode = h
A = Astar()
ans,iter_count,front_len,visited = A.a_star(start,target,mode)
print "____Some Statistics____"
print "Heuristic :",heurstics[h]
print "Max frontier length",front_len
print "Length of path",len(ans)
print "Number of iterations",iter_count
print "Visited States",visited
print "Solutio path:"
Game.print_moves(ans[::-1])
#if ans:
# print_moves(ans[::-1])
#else:
# print "No Solution exists"
#print_moves(start.successor())
#print_moves(successor(game))
def execute_algo(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
# BFS
if self.algorithm_state == 'bfs':
self.bfs = BreadthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.bfs.bfs_execute()
if self.bfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.bfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DFS
elif self.algorithm_state == 'dfs':
self.dfs = DepthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dfs.dfs_execute()
if self.dfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.dfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# ASTAR
elif self.algorithm_state == 'astar':
self.astar = Astar(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.astar.astar_execute()
if self.astar.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.astar.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DIJKSTRA
elif self.algorithm_state == 'dijkstra':
self.dijkstra = Dijkstra(self, self.start_node_x,
self.start_node_y, self.end_node_x,
self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dijkstra.dijkstra_execute()
if self.dijkstra.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.dijkstra.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
pygame.display.update()
self.state = 'aftermath'
class App:
def __init__(self):
self.screen = pygame.display.set_mode((WIDTH, HEIGHT))
self.clock = pygame.time.Clock()
self.running = True
self.state = 'main menu'
self.algorithm_state = ''
self.grid_square_length = 24
# self.load()
self.start_end_checker = 0
self.mouse_drag = 0
# start and end nodes coordinates
self.start_node_x = None
self.start_node_y = None
self.end_node_x = None
self.end_node_y = None
# wall nodes list
self.wall_pos = wall_nodes_coords_list.copy()
# main menu buttons
self.bfs_button = Button(self, WHITE, 338, MAIN_BUTTON_Y_POS,
MAIN_BUTTON_LENGTH, MAIN_BUTTON_HEIGHT, 'BFS')
self.dfs_button = Button(self, WHITE, 558, MAIN_BUTTON_Y_POS,
MAIN_BUTTON_LENGTH, MAIN_BUTTON_HEIGHT, 'DFS')
self.astar_button = Button(self, WHITE, 778, MAIN_BUTTON_Y_POS,
MAIN_BUTTON_LENGTH, MAIN_BUTTON_HEIGHT,
'Astar')
self.dijkstra_button = Button(self, WHITE, 998, MAIN_BUTTON_Y_POS,
MAIN_BUTTON_LENGTH, MAIN_BUTTON_HEIGHT,
'Dijkstra')
# grid menu options
self.start_end_node_button = Button(self, AQUAMARINE, 20,
START_END_BUTTON_HEIGHT,
GRID_BUTTON_LENGTH,
GRID_BUTTON_HEIGHT,
'Start/End Node')
self.wall_node_button = Button(
self, AQUAMARINE, 20,
START_END_BUTTON_HEIGHT + GRID_BUTTON_HEIGHT + BUTTON_SPACER,
GRID_BUTTON_LENGTH, GRID_BUTTON_HEIGHT, 'Wall Node')
self.reset_button = Button(
self, AQUAMARINE, 20, START_END_BUTTON_HEIGHT +
GRID_BUTTON_HEIGHT * 2 + BUTTON_SPACER * 2, GRID_BUTTON_LENGTH,
GRID_BUTTON_HEIGHT, 'RESET')
self.start_button = Button(
self, AQUAMARINE, 20, START_END_BUTTON_HEIGHT +
GRID_BUTTON_HEIGHT * 3 + BUTTON_SPACER * 3, GRID_BUTTON_LENGTH,
GRID_BUTTON_HEIGHT, 'Visualize')
self.main_menu_button = Button(
self, AQUAMARINE, 20, START_END_BUTTON_HEIGHT +
GRID_BUTTON_HEIGHT * 4 + BUTTON_SPACER * 4, GRID_BUTTON_LENGTH,
GRID_BUTTON_HEIGHT, 'Main Menu')
def run(self):
while self.running:
if self.state == 'main menu':
self.main_menu_events()
if self.state == 'grid window':
self.grid_events()
if self.state == 'draw S/E' or self.state == 'draw walls':
self.draw_nodes()
if self.state == 'start visualizing':
self.execute_algo()
if self.state == 'aftermath':
self.reset_or_main_menu()
pygame.quit()
sys.exit()
def load(self):
# self.main_menu_background = pygame.image.load('background.png')
# self.grid_background = pygame.image.load('grid.png')
pass
@staticmethod
def draw_text(words, screen, pos, size, color, font_name, center=False):
font = pygame.font.SysFont(font_name, size)
text = font.render(words, False, color)
text_size = text.get_size()
if center:
pos[0] = pos[0] - text_size[0] // 2
pos[1] = pos[1] - text_size[1] // 2
screen.blit(text, pos)
def sketch_main_menu(self):
# draw background
# self.screen.blit(self.main_menu_background, (0, 0))
self.bfs_button.draw_button(AQUAMARINE)
self.dfs_button.draw_button(AQUAMARINE)
self.astar_button.draw_button(AQUAMARINE)
self.dijkstra_button.draw_button(AQUAMARINE)
def sketch_hotbar(self):
self.screen.fill(BLACK)
pygame.draw.rect(self.screen, WHITE, (0, 0, 240, 768), 0)
# draw grid background
# self.screen.blit(self.grid_background, (0, 0))
def sketch_grid(self):
# add borders
pygame.draw.rect(self.screen, ALICE, (240, 0, WIDTH, HEIGHT), 0)
pygame.draw.rect(self.screen, AQUAMARINE,
(264, 24, GRID_WIDTH, GRID_HEIGHT), 0)
# draw grid with size 52, 30
for x in range(52):
pygame.draw.line(self.screen, ALICE,
(GS_X + x * self.grid_square_length, GS_Y),
(GS_X + x * self.grid_square_length, GE_Y))
for y in range(30):
pygame.draw.line(self.screen, ALICE,
(GS_X, GS_Y + y * self.grid_square_length),
(GE_X, GS_Y + y * self.grid_square_length))
def sketch_grid_buttons(self):
# draw buttons
self.start_end_node_button.draw_button(STEELBLUE)
self.wall_node_button.draw_button(STEELBLUE)
self.reset_button.draw_button(STEELBLUE)
self.start_button.draw_button(STEELBLUE)
self.main_menu_button.draw_button(STEELBLUE)
def set_menu_buttons_color(self, color):
self.start_end_node_button.color = color
self.wall_node_button.color = color
self.reset_button.color = color
self.start_button.color = color
self.main_menu_button.color = color
def grid_window_buttons(self, pos, event):
if event.type == pygame.MOUSEBUTTONDOWN:
if self.start_end_node_button.isOver(pos):
self.state = 'draw S/E'
elif self.wall_node_button.isOver(pos):
self.state = 'draw walls'
elif self.reset_button.isOver(pos):
self.execute_reset()
elif self.start_button.isOver(pos):
self.state = 'start visualizing'
elif self.main_menu_button.isOver(pos):
self.back_to_menu()
if event.type == pygame.MOUSEMOTION:
if self.start_end_node_button.isOver(pos):
self.start_end_node_button.color = MINT
elif self.wall_node_button.isOver(pos):
self.wall_node_button.color = MINT
elif self.reset_button.isOver(pos):
self.reset_button.color = MINT
elif self.start_button.isOver(pos):
self.start_button.color = MINT
elif self.main_menu_button.isOver(pos):
self.main_menu_button.color = MINT
else:
self.set_menu_buttons_color(STEELBLUE)
def grid_button_keep_color(self):
if self.state == 'draw S/E':
self.start_end_node_button.color = MINT
elif self.state == 'draw walls':
self.wall_node_button.color = MINT
def execute_reset(self):
self.start_end_checker = 0
# reset start and end nodes coordinates
self.start_node_x = None
self.start_node_y = None
self.end_node_x = None
self.end_node_y = None
# reset wall nodes list
self.wall_pos = wall_nodes_coords_list.copy()
# reset state
self.state = 'grid window'
def back_to_menu(self):
self.start_end_checker = 0
# reset start and end nodes coordinates
self.start_node_x = None
self.start_node_y = None
self.end_node_x = None
self.end_node_y = None
# reset wall nodes list
self.wall_pos = wall_nodes_coords_list.copy()
# switch state
self.state = 'main menu'
def set_algo_buttons_color(self, color):
self.bfs_button.color = color
self.dfs_button.color = color
self.astar_button.color = color
self.dijkstra_button.color = color
def main_menu_events(self):
# draw background
pygame.display.update()
self.sketch_main_menu()
self.draw_text('Path Finding',
self.screen, [1200, 720],
28,
WHITE,
FONT,
center=False)
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
pos = pygame.mouse.get_pos()
if event.type == pygame.MOUSEBUTTONDOWN:
if self.bfs_button.isOver(pos):
self.algorithm_state = 'bfs'
self.state = 'grid window'
if self.dfs_button.isOver(pos):
self.algorithm_state = 'dfs'
self.state = 'grid window'
if self.astar_button.isOver(pos):
self.algorithm_state = 'astar'
self.state = 'grid window'
if self.dijkstra_button.isOver(pos):
self.algorithm_state = 'dijkstra'
self.state = 'grid window'
if event.type == pygame.MOUSEMOTION:
if self.bfs_button.isOver(pos):
self.bfs_button.color = AQUAMARINE
elif self.dfs_button.isOver(pos):
self.dfs_button.color = AQUAMARINE
elif self.astar_button.isOver(pos):
self.astar_button.color = AQUAMARINE
elif self.dijkstra_button.isOver(pos):
self.dijkstra_button.color = AQUAMARINE
else:
self.set_algo_buttons_color(WHITE)
def grid_events(self):
self.sketch_hotbar()
self.sketch_grid()
self.sketch_grid_buttons()
pygame.display.update()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
pos = pygame.mouse.get_pos()
self.grid_window_buttons(pos, event)
def draw_nodes(self):
self.grid_button_keep_color()
self.sketch_grid_buttons()
pygame.display.update()
pos = pygame.mouse.get_pos()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
self.grid_window_buttons(pos, event)
if 264 < pos[0] < 1512 and 24 < pos[1] < 744:
x_grid_pos = (pos[0] - 264) // 24
y_grid_pos = (pos[1] - 24) // 24
if event.type == pygame.MOUSEBUTTONDOWN:
self.mouse_drag = 1
if self.state == 'draw S/E' and self.start_end_checker < 2:
# color and coordinates for start node
if self.start_end_checker == 0:
node_color = TOMATO
self.start_node_x = x_grid_pos + 1
self.start_node_y = y_grid_pos + 1
self.start_end_checker += 1
# color and coordinates for end node, making sure it's different from start node
elif self.start_end_checker == 1 \
and (x_grid_pos + 1, y_grid_pos + 1) != (self.start_node_x, self.start_node_y):
node_color = ROYALBLUE
self.end_node_x = x_grid_pos + 1
self.end_node_y = y_grid_pos + 1
self.start_end_checker += 1
else:
continue
# draw on the grid
pygame.draw.rect(self.screen, node_color,
(264 + x_grid_pos * 24,
24 + y_grid_pos * 24, 24, 24), 0)
elif event.type == pygame.MOUSEBUTTONUP:
self.mouse_drag = 0
# check to see if mouse drag is available to draw wall nodes
if self.mouse_drag == 1:
# draw wall nodes and append them accordingly
# check for duplicates in the list and if they are overlapping start/end nodes
if self.state == 'draw walls':
if (x_grid_pos + 1, y_grid_pos + 1) not in self.wall_pos \
and (x_grid_pos + 1, y_grid_pos + 1) != (self.start_node_x, self.start_node_y) \
and (x_grid_pos + 1, y_grid_pos + 1) != (self.end_node_x, self.end_node_y):
pygame.draw.rect(self.screen, BLACK,
(264 + x_grid_pos * 24,
24 + y_grid_pos * 24, 24, 24), 0)
self.wall_pos.append(
(x_grid_pos + 1, y_grid_pos + 1))
for x in range(52):
pygame.draw.line(
self.screen, ALICE,
(GS_X + x * self.grid_square_length, GS_Y),
(GS_X + x * self.grid_square_length, GE_Y))
for y in range(30):
pygame.draw.line(
self.screen, ALICE,
(GS_X, GS_Y + y * self.grid_square_length),
(GE_X, GS_Y + y * self.grid_square_length))
def execute_algo(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
# BFS
if self.algorithm_state == 'bfs':
self.bfs = BreadthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.bfs.bfs_execute()
if self.bfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.bfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DFS
elif self.algorithm_state == 'dfs':
self.dfs = DepthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dfs.dfs_execute()
if self.dfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.dfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# ASTAR
elif self.algorithm_state == 'astar':
self.astar = Astar(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.astar.astar_execute()
if self.astar.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.astar.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DIJKSTRA
elif self.algorithm_state == 'dijkstra':
self.dijkstra = Dijkstra(self, self.start_node_x,
self.start_node_y, self.end_node_x,
self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dijkstra.dijkstra_execute()
if self.dijkstra.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.dijkstra.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
pygame.display.update()
self.state = 'aftermath'
def reset_or_main_menu(self):
self.sketch_grid_buttons()
pygame.display.update()
pos = pygame.mouse.get_pos()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
if event.type == pygame.MOUSEMOTION:
if self.start_end_node_button.isOver(pos):
self.start_end_node_button.color = MINT
elif self.wall_node_button.isOver(pos):
self.wall_node_button.color = MINT
elif self.reset_button.isOver(pos):
self.reset_button.color = MINT
elif self.start_button.isOver(pos):
self.start_button.color = MINT
elif self.main_menu_button.isOver(pos):
self.main_menu_button.color = MINT
else:
self.set_menu_buttons_color(STEELBLUE)
if event.type == pygame.MOUSEBUTTONDOWN:
if self.reset_button.isOver(pos):
self.execute_reset()
elif self.main_menu_button.isOver(pos):
self.back_to_menu()
def runAlgorithm(algo):
file = open(inputPath,"r")
afile = open("analysis.csv","a")
afile.write("\nAlgorithm, Puzzle, Cost, Length Of Solution, Length Of Search, Execution, No Solution, Optimality\n")
goalstate1 = "1 2 3 4 5 6 7 0"
goalstate2 = "1 3 5 7 2 4 6 0"
totalLengthOfSolution = 0
totalLengthOfSearch = 0
totalNoSolution = 0
totalCost = 0
totalExecutionTime = 0
counter = 0
averageCost = averageExecutionTime = averageLengthOfSearch = averageLengthOfSolution = averageNoSolution = 0
i = 0
for line in file:
lengthOfSolution = 0
lengthOfSearch = 0
duration = 0
cost = 0
analysis = None
if algo == ucs:
analysis = algoAnalysis(UniformCostSearch(puzzleNumber = i, initial = line.strip(), goal_state1 = goalstate1,goal_state2 = goalstate2))
elif algo == gbfsH0:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h0", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH1:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h1", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH2:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h2", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH3:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h3", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == astarH0:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h0", goalState1=goalstate1, goalState2=goalstate2, monotonic=False))
elif algo == astarH1:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h1", goalState1=goalstate1, goalState2=goalstate2, monotonic = False))
elif algo == astarH2:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h2", goalState1=goalstate1, goalState2=goalstate2, monotonic=True))
elif algo == astarH3:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h3", goalState1=goalstate1, goalState2=goalstate2, monotonic=False))
duration = analysis[5]
lengthOfSolution = analysis[1]
lengthOfSearch = analysis[2]
if analysis[3] == 0:
cost = analysis[4]
counter = counter + 1
totalCost = totalCost+cost
totalExecutionTime = totalExecutionTime + duration
totalLengthOfSearch = totalLengthOfSearch + lengthOfSearch
if lengthOfSolution != 0:
totalLengthOfSolution = totalLengthOfSolution + lengthOfSolution
else:
totalNoSolution = totalNoSolution + 1
afile.write(algo +", "+ str(i)+", "+str(cost)+", "+str(lengthOfSolution)+", "+str(lengthOfSearch)+", "+str(duration)+","+str(analysis[3])+",,"+line.strip()+"\n")
i = i + 1
afile.write("Total-"+algo+", , "+str(totalCost)+","+str(totalLengthOfSolution)+", "+str(totalLengthOfSearch)+", "+str(totalExecutionTime)+","+str(totalNoSolution)+"\n")
if(i != 0 and counter != 0):
averageCost = totalCost/counter
averageExecutionTime = totalExecutionTime/counter
averageLengthOfSearch = totalLengthOfSearch/i
averageLengthOfSolution = totalLengthOfSolution/counter
averageNoSolution = totalNoSolution/i
afile.write("Average-"+algo+", , "+str(averageCost)+","+str(averageLengthOfSolution)+", "+str(averageLengthOfSearch)+", "+str(averageExecutionTime)+","+str(averageNoSolution)+"\n")
file.close()
afile.close()
# -*- coding: utf-8 -*-
from environment import Environment
from dijkstra import Dijkstra
from astar import Astar
if __name__ == "__main__":
resolution = 0.1
botRadius = 0
env = Environment(resolution, botRadius)
env.render()
alg = Dijkstra(env)
alg.executeAlg()
env.reset()
alg1 = Astar(env)
alg1.executeAlg()
class Snake:
# Initializing Snake
def __init__(self, grid: Grid):
# Variables
self.body = []
self.body_positions = []
self.dead = False
self.won = False
self.speed = grid.scl
# Environment Variables
self.grid = grid
self.cycle = grid.hCycle
self.cyclePath = None
# A* Variables
self.getNextAstarPath = False
self.astarPath = None
self.gettingAstarPath = False
# Internal Measurements
self.frame = 0
self.gainFromFood = 4
# Growth Measurements
self.growthLength = self.gainFromFood
self.drawnLength = 0
# Snake Head
self.directions = ['up', 'down', 'left', 'right']
self.head = turtle.Turtle()
self.head.pu()
self.head.shape('scaled-square')
self.color = '#009600'
self.head.color(self.color)
self.head.width(self.speed - (self.grid.bord * 2) + 1)
self.head.origin = self.grid.center()
self.head.goto(self.head.origin)
self.head.dir = 'right'
# Snake Food
self.food = turtle.Turtle()
self.food.pu()
self.food.shape('scaled-square')
self.food.color('red')
self.setFoodPos()
# Snake Movement
def up(self):
if self.head.dir != 'down':
self.head.dir = 'up'
def down(self):
if self.head.dir != 'up':
self.head.dir = 'down'
def left(self):
if self.head.dir != 'right':
self.head.dir = 'left'
def right(self):
if self.head.dir != 'left':
self.head.dir = 'right'
# Moving Snake Head and Checking Food
def update(self):
# Snake Border
self.head.clear()
if len(self.body) > 0:
self.head.pd()
# Snake Directional Movement
if self.head.dir == 'up':
self.head.sety(self.head.ycor() + self.speed)
elif self.head.dir == 'down':
self.head.sety(self.head.ycor() - self.speed)
elif self.head.dir == 'left':
self.head.setx(self.head.xcor() - self.speed)
elif self.head.dir == 'right':
self.head.setx(self.head.xcor() + self.speed)
# Checking Wall Hits
if self.head.pos() not in self.grid.cells:
self.die()
# Getting all Snake Body Positions
self.getBodyPositions()
if len(self.body) >= self.grid.size - 2:
self.won = True
# Checking if Snake got to Food
if self.head.pos() == self.food.pos():
self.setFoodPos()
self.growthLength += self.gainFromFood
if self.gettingAstarPath:
self.getNextAstarPath = True
# Adding Body Segments from gain variable
if self.growthLength > 0:
if len(self.body) > 0:
self.body[-1].pd()
self.addSegment()
self.growthLength -= 1
self.drawnLength += 1
# Snake internal clock
self.frame += 1
# Set Food Position to a Spot that the body is not on
def setFoodPos(self):
position = self.grid.randomPos()
# Repeating until a viable spot is found
while position in self.getBodyPositions() or position == self.head.pos(
):
position = self.grid.randomPos()
# Setting Food Position
self.food.goto(position)
# Moving Snake Body Segments
def move(self):
# Running through Snake Body Backwards
for index in range(len(self.body) - 1, 0, -1):
self.body[index].clear()
self.body[index].st()
# Moving body segment
x = self.body[index - 1].xcor()
y = self.body[index - 1].ycor()
self.body[index].goto(x, y)
if index != len(self.body) - 1:
self.body[index].pd()
# Moving First body Segment
if len(self.body) > 0:
self.body[0].clear()
self.body[0].st()
# Moving to head positions
x = self.head.xcor()
y = self.head.ycor()
self.body[0].goto(x, y)
if len(self.body) > 1:
self.body[0].pd()
# Adding a Segment
def addSegment(self):
# Generating new segment
new_segment = turtle.Turtle()
new_segment.speed(0)
new_segment.width(self.speed - (self.grid.bord * 2) + 1)
new_segment.ht()
new_segment.shape('scaled-square')
new_segment.color(self.color)
new_segment.pu()
new_segment.goto(self.head.pos())
# Adding the new segment to the body
self.body.append(new_segment)
# Checking if the head collides with any part of the tail
def checkCollisionWithBody(self):
if self.head.pos() in self.body_positions:
self.die()
# Checking Collision with position in predicted walls
def checkCollision(self, pos, walls):
if pos in walls or pos not in self.grid.cells:
return True
return False
# Getting All positions of snake body
def getBodyPositions(self):
positions = []
for seg in self.body:
positions.append(seg.pos())
self.body_positions = positions
return positions
# Compiling all functions into one run function
def run(self):
# Only Move if the game is not won or lost
if not self.won and not self.dead:
self.move() # Moving Body Segments
self.update() # Moving Head
self.checkCollisionWithBody() # Checking if Dead
# Hiding Snake
def hide(self):
self.head.ht()
self.head.clear()
self.food.ht()
for seg in self.body:
seg.clear()
seg.ht()
# Showing Snake
def show(self):
self.head.st()
self.food.st()
for seg in self.body:
seg.st()
# Destroying Snake
def die(self):
self.dead = True
self.hide()
del self
# ALGORITHMS ----------------------------------------------------------------------------------------------
# Moving to a node
def toward(self, pos):
x, y = pos
if x == self.head.xcor() and y > self.head.ycor(): self.up()
elif x == self.head.xcor() and y < self.head.ycor(): self.down()
elif x > self.head.xcor() and y == self.head.ycor(): self.right()
elif x < self.head.xcor() and y == self.head.ycor(): self.left()
# A* Compiling Algorithm
def getPathFromAstar(self):
self.gettingAstarPath = True
if self.astarPath == None or self.getNextAstarPath:
self.astarPath = Astar(self.head.pos(), self.food.pos(),
self.getBodyPositions(), self.grid)
self.getNextAstarPath = False
try:
self.toward(self.astarPath[self.astarPath.index(self.head.pos()) +
1])
except:
ValueError
return self.astarPath
# Path Distance from nodes
def pathDistance(self, pos1, pos2):
if pos1 < pos2:
return pos2 - pos1 - 1
return pos2 - pos1 - 1 + self.grid.size
# Calculating Path From Head to food
def getPathFromShortcutHamilton(self):
layer = 0
path = [self.getNextMoveFromShortcut(self.head.pos())]
while path[-1] != self.food.pos():
layer += 1
nextNode = self.getNextMoveFromShortcut(path[-1], layer, path)
path.append(nextNode)
return path
# Main Movement Function for Shortcut Algorithm
def pathShortcutHamilton(self):
self.cyclePath = self.getPathFromShortcutHamilton()
self.toward(self.cyclePath[0])
return self.cyclePath
# Only Calculating One Block Ahead
def shortcutHamilton(self):
node = self.getNextMoveFromShortcut(self.head.pos())
self.toward(node)
return [node]
def getNextMoveFromShortcut(self, pos, layer=1, pathSoFar=[]):
# Predicting Body Movement with shortcut
walls = self.getBodyPositions()
x, y = pos
if len(walls) - layer > 0:
for _ in range(layer):
walls.remove(walls[-1])
else:
walls.clear()
walls = [pos] + pathSoFar + walls
# Getting Main Distances
pathNumber = self.cycle.index(pos)
distanceToFood = self.pathDistance(pathNumber,
self.cycle.index(self.food.pos()))
if len(self.body) > 0:
distanceToTail = self.pathDistance(pathNumber,
self.cycle.index(walls[-1]))
else:
distanceToTail = float('inf')
# Calculating Available Cutting Amounts
cuttingAmountAvailable = distanceToTail - self.drawnLength - self.gainFromFood - self.growthLength
emptySquaresOnBoard = self.grid.size - self.drawnLength - self.growthLength - 1
if self.drawnLength >= self.grid.size * 0.5:
cuttingAmountAvailable = 0 # Disabling Shortcuts
elif distanceToFood < distanceToTail:
cuttingAmountAvailable -= (self.gainFromFood + self.growthLength)
if (distanceToTail - distanceToFood) * 4 > emptySquaresOnBoard:
cuttingAmountAvailable -= (self.gainFromFood +
self.growthLength)
# Normalizing Cutting Amounts
cuttingAmountDesired = distanceToFood
if cuttingAmountDesired < cuttingAmountAvailable:
cuttingAmountAvailable = cuttingAmountDesired
if cuttingAmountAvailable < 0:
cuttingAmountAvailable = 0
# Directions
above = (x, y + self.speed)
below = (x, y - self.speed)
onleft = (x - self.speed, y)
onright = (x + self.speed, y)
# Checking if Direction is safe
canGoRight = not self.checkCollision(onright, walls)
canGoLeft = not self.checkCollision(onleft, walls)
canGoUp = not self.checkCollision(above, walls)
canGoDown = not self.checkCollision(below, walls)
# Safety Lookup Table
canGo = {
above: canGoUp,
below: canGoDown,
onright: canGoRight,
onleft: canGoLeft
}
# Main Comparison Items
bestDir = ()
bestDist = -1
# Possible Directions
goList = [above, below, onleft, onright]
# Looking for optimal node to travel to
for node in goList:
if canGo[node]:
dist = self.pathDistance(pathNumber, self.cycle.index(node))
if dist <= cuttingAmountAvailable and dist > bestDist:
bestDir = node
bestDist = dist
# Return Best Direction
if bestDist >= 0:
return bestDir
# Should not reach here -------------------
print(random.random())
# Returning viable node
for node in goList:
if canGo[node]:
return node
return above
def run_astar(episode):
MAX_RUNS = 2000000
TURN_ANGLE = 30
FORWARD_STEP_SIZE = 0.25
AGENT_HEIGHT = 0.88
AGENT_RADIUS = 0.18
house_floor_threshold = {
'2azQ1b91cZZ': [1.5],
'8194nk5LbLH': [1.2],
'EU6Fwq7SyZv': [-1.3, 2.0],
'oLBMNvg9in8': [-0.8, 1.8, 4.6],
'pLe4wQe7qrG': [],
'QUCTc6BB5sX': [-1.5],
'TbHJrupSAjP': [-1.5, 1.7],
'X7HyMhZNoso': [1.8],
'x8F5xyUWy9e': [],
'Z6MFQCViBuw': [],
'zsNo4HB9uLZ': [],
}
# -- folders
data_dir = '../data/ObjectNav/objectnav_mp3d_v1/val/'
folder_pred = '../data/ObjectNav/semmap/'
floormap_dir = '../data/ObjectNav/freespace_map/'
info = json.load(open('../data/ObjectNav/semmap_objnav_info.json', 'r'))
# -- setup naming bindings
read_tsv = csv.reader(open("mpcat40.tsv"), delimiter="\t")
mpcat40 = {line[0]: line[1] for line in read_tsv}
jsonfile = json.load(open(os.path.join(data_dir, 'val.json'), 'r'))
category_to_mp3d_category_id = jsonfile['category_to_mp3d_category_id']
house = episode['scene_id'].split('/')[1]
# -- sample object category target
obj_semantic_id = category_to_mp3d_category_id[episode['object_category']]
obj_semantic_name = mpcat40[str(obj_semantic_id)]
if obj_semantic_name in object_whitelist:
sid = object_whitelist.index(obj_semantic_name) + 1
else:
return (episode['episode_id'], [], [])
# -- select floor -> env
if house_floor_threshold[house]:
level = bisect.bisect(house_floor_threshold[house],
episode['start_position'][1])
else:
level = 0
env = house + '_' + str(level)
map_world_shift = np.array(info[env]['map_world_shift'])
# -- load maps
floormap = imread(os.path.join(floormap_dir, env + '.png'))
floormap = floormap.astype(np.float)
floormap /= 255
floormap = floormap.astype(np.bool)
if not os.path.isfile(os.path.join(folder_pred, env + '.h5')):
return (episode['episode_id'], [], [])
h5file = h5py.File(os.path.join(folder_pred, env + '.h5'), 'r')
map_semantic = np.array(h5file['semmap'])
observed_map = np.array(h5file['observed_map'])
observed_map = observed_map.astype(np.float)
h5file.close()
map_semantic = np.multiply(map_semantic, observed_map)
map_semantic = map_semantic.astype(np.int)
#goals = None
goals = all_goals[house][episode['episode_id']]
goals = np.array(goals)
goals -= map_world_shift
goals = goals[:, [0, 2]]
# -- get init position
start_pos = episode['start_position']
start_pos -= map_world_shift
start_x = start_pos[0]
start_y = start_pos[2]
start_row = int(np.round(start_y / resolution))
start_col = int(np.round(start_x / resolution))
start_rot = np.array(episode['start_rotation'])
r = R.from_quat(start_rot)
_, start_heading, _ = r.as_rotvec()
start_heading = (start_heading * 180 / np.pi)
start_heading = -(start_heading + 90)
start = Node(x=start_x,
y=start_y,
heading=start_heading,
row=start_row,
col=start_col)
# -- get maps for Astar
goal_mask = map_semantic == sid
goal_mask = binary_opening(goal_mask.astype(int),
structure=np.ones((3, 3))).astype(np.bool)
floormap = binary_closing(floormap.astype(int),
structure=np.ones((10, 10))).astype(np.bool)
navmap = floormap & (map_semantic == 0)
# compute Heuristic
# -- Euclidean distance
non_object_mask = ~goal_mask
non_object_mask = non_object_mask.astype(np.uint8)
distance_map = cv2.distanceTransform(non_object_mask.copy(), cv2.DIST_L2,
3)
# -- Geodesic distance
#os.system('pip install scikit-fmm')
#import skfmm
#import numpy.ma as ma
#mask = ~navmap&~goal_mask
#mask = ~mask
#mask = binary_dilation(mask.astype(int), structure=np.ones((10,10))).astype(np.bool)
#mask = ~mask
#map = np.ones(navmap.shape)
#map = map - 2*goal_mask.astype(np.float)
#map = ma.masked_array(map, mask)
#distance_map = skfmm.distance(map)
pathfinder = Astar(navmap,
observed_map,
heuristic=distance_map,
init_heading=start_heading,
goals=goals)
path, runs = pathfinder.run(start, goal_mask, max_runs=MAX_RUNS)
if len(path) == 0:
actions = []
else:
path = path[::-1]
# -- convert path to actions
actions = []
prev_p = path[0]
for p in path[1:]:
# -- rotate
pre_h = prev_p[2]
new_h = p[2]
delta_h = (new_h - pre_h + 360) % 360
if delta_h == 0:
pass
elif delta_h <= 180:
#trun right
num_rotations = int(delta_h / TURN_ANGLE)
actions += [3] * abs(num_rotations)
elif delta_h > 180:
#trun left
delta_h = 360 - delta_h
num_rotations = int(delta_h / TURN_ANGLE)
actions += [2] * abs(num_rotations)
# move forward
actions.append(1)
prev_p = p
actions.append(0)
return (episode['episode_id'], actions, path)
def initialize(self, specs):
# Create start end goal states
start = NodeState(specs.get('start'), None, specs.get('board'), specs.get('goal'))
# Set the algorithm, with start and goal
self.algorithm = Astar(start_state=start, sorting=self.algorithm_choice)
class Run:
# Constructor
def __init__(self, gui):
self.gui = gui
# Algorithm to use, default = 0 -> astar
self.algorithm_choice = 0
self.algorithm = None
# Set algorithm
def set_algorithm(self, type):
self.algorithm_choice = type
if self.algorithm:
self.algorithm.sorting = self.algorithm_choice
def open_file(self, level):
self.initialize(self.generate_board(level))
def initialize(self, specs):
# Create start end goal states
start = NodeState(specs.get('start'), None, specs.get('board'), specs.get('goal'))
# Set the algorithm, with start and goal
self.algorithm = Astar(start_state=start, sorting=self.algorithm_choice)
def __delete__(self, instance):
print 'del run'
del self.algorithm
# Run algorithm
def run(self):
# Calculate scores
self.algorithm.do_next(self.gui)
## ----------------------------------
## File and Directory operations
## ----------------------------------
# Read all avaliable boards
def list_files(self):
levels = []
os.chdir("boards")
for file in glob.glob("*.txt"):
if file:
levels.append(file)
return levels
# Read specific file
def read_file(self, filename):
with open(os.path.dirname(os.path.realpath(__file__)) + "/boards/" + filename, "r") as myfile:
data = myfile.read()
return data
# Save a new level file
def save_file(self, filename, string_file):
text_file = open("boards/" + filename, "w")
text_file.write(string_file)
text_file.close()
# Create a board from text
def generate_board(self, filename=None, str_level=None):
board = None
if str_level is not None:
raw_board = str_level
else:
raw_board = self.read_file(filename)
board_config = raw_board.split('\n')
# Initialize board
board_size = board_config[0].split(' ')
board = [[' ' for y in range(int(board_size[1]))] for x in range(int(board_size[0]))]
# Start and Goal
board_start = [int(board_config[1].split(' ')[0]), int(board_config[1].split(' ')[1])]
board_goal = [int(board_config[1].split(' ')[2]), int(board_config[1].split(' ')[3])]
board[board_start[0]][board_start[1]] = 'S'
board[board_goal[0]][board_goal[1]] = 'G'
# Constraints, 2 first lines are size and start, goal
for i in range(2, len(board_config)):
constraint_line = board_config[i].split(' ')
constraint_origo = [int(constraint_line[0]), int(constraint_line[1])]
constraint_dimen = [int(constraint_line[2]), int(constraint_line[3])]
for cx in range(constraint_origo[0], constraint_dimen[0] + constraint_origo[0]):
for cy in range(constraint_origo[1], constraint_dimen[1] + constraint_origo[1]):
board[cx][cy] = '#'
board_specs = {'board': board, 'start': board_start, 'goal': board_goal}
return board_specs
use_astar = True
elif op in ("-d", "--dstar"):
use_dstar = True
if use_astar is None and use_dstar is None:
print("Error: You need to select one algorithm to plan path: Astar or Dstar!")
sys.exit()
else:
print('Space: ', space_boundary)
print('Agents number: ', agents_num)
print('Safe Radius: ', safe_R)
print('Seed: ', seed)
env = Env(space_boundary = space_boundary,
agents_num=agents_num, seed = seed, safe_R = safe_R)
if use_astar:
print('Algorithm: Astar')
astar = Astar(env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos,
env.space_boundary, env.walk_dirs)
pathData = astar.search()
pathsData = {"Plan Path": pathData}
draw_path(env.space_boundary ,env.agents_pos, pathsData,
env.agents_targ, title = "Path Plan with A*")
elif use_dstar:
print('Algorithm: Dstar')
space_map = Map(env.space_boundary, env.walk_dirs)
dstar = Dstar(space_map, env.space_boundary)
paths = dstar.search( env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos)
pathsData = {"Path Without Obstacle": paths[0], "Path With Obstacle": paths[1]}
draw_path(env.space_boundary ,env.agents_pos, pathsData,
env.agents_targ, title = "Path Plan with D*")
print("Finish!")
sys.exit()
altitude, safe_distance = 5, 3
self.grid = create_grid(data, altitude, safe_distance)
def get_grid(self):
return self.grid
def show(self, x_values, y_values):
skeleton = medial_axis(invert(self.grid))
plt.imshow(self.grid, origin='lower')
plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7)
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.plot(x_values, y_values, 'g')
plt.show()
start_ne = (25, 100)
goal_ne = (750., 370.)
medial = Medial('colliders.csv')
grid = medial.get_grid()
astar = Astar(grid)
found, paths = astar.travel(start_ne, goal_ne)
path = astar.trace_back(paths) if found else exit("Couldn't find a path")
xpoints, ypoints = astar.axis_points(path)
medial.show(xpoints, ypoints)
numlist = line.split(' ')
if len(numlist) < 15:
return
problems.append(Puzzle([int(x) for x in numlist[1:]]))
print('%5s%10s%10s%10s%10s' % ('#prob', '#exp', '#gen', '|sol|', 'tiempo'))
problems = []
load_problems(problems)
total_time = 0
total_cost = 0
total_expansions = 0
total_problems = len(problems) # cambiar si quieres menos problemas
for prob in range(0, total_problems):
init = problems[prob] # problema aleatorio
s = Astar(init, heuristic, 1)
result = s.search()
print('%5d%10d%10d%10d%10.2f' % (prob + 1, s.expansions, len(
s.generated), result.g, s.end_time - s.start_time))
total_time += s.end_time - s.start_time
total_expansions += s.expansions
total_cost += result.g
if show_solutions:
print(result.trace())
print('Total time: %.3f' % (total_time))
print('Expansiones totales: %d' % (total_expansions))
print('Total cost: %.3d' % (total_cost))
def setUp(self):
self.searcher1 = Astar((0, 0), (10, 10), 10)
self.searcher2 = Astar((-5, -5), (5, 5), 10)
self.searcher3 = Astar((0, 0), (10, 10), 100)
import sys
from map import Map
from astar import Astar
if __name__ == '__main__':
size = len(sys.argv)
if size == 2:
try:
_file = open(sys.argv[1], "r")
content = _file.read()
_file.close()
map = Map(content)
except:
print("The file is unknown")
if map.getStepNumber() >= 2:
Astar(map)
else:
print("The map must have 2 steps at least")
else:
print("The path of the file is required")
class Probleminstance():
'''
'''
def __init__(self, domains, constraint_list):
'''
Initializes the values used by astar and gac.
'''
#A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
#GAC INFO
self.constraints = Constraints("x!=y", ["x", "y"], constraint_list)
self.domains = domains
#init gac
self.gac = GAC()
def initialize(self):
'''
Initializes by running the first domain filtering loop.
'''
self.gac.initialize(self.domains, self.constraints)
self.domains = self.gac.domain_filtering_loop()
self.astar = Astar(self)
def solve(self):
'''
Runs one iteration of the astar algorithm
'''
self.current = self.astar.solve("A*")
return [self, self.current, self.astar.prev_current]
def is_solution(self):
'''
Returns True if this is a solution state, False if not.
'''
for domain in self.domains:
if len(self.domains[domain]) != 1:
return False
return True
def get_neighbours(self):
'''
Returns the neighbours of this node.
'''
minlen = float("inf")
current_domain = None
neighbours = list()
for domain in range(len(self.domains)):
if len(self.domains[domain]) == 1:
continue
if (len(self.domains[domain])) < minlen:
minlen = len(self.domains[domain])
current_domain = domain
for color in self.domains[current_domain]:
copy_domains = copy.deepcopy(self.domains)
copy_domains[current_domain] = [color]
copy_domains = self.gac.rerun(copy_domains, current_domain, self.constraints)
pi = Probleminstance(copy_domains, self.constraints.involved)
neighbours.append(pi)
self.neighbours = neighbours
return neighbours
def get_arc_cost(self):
'''
Returns the cost of moving from this node.
'''
return 1
def get_h(self):
'''
Sets and returns the h value
'''
h = 0
for domain in self.domains:
h += len(self.domains[domain]) - 1
self.h = h
return self.h
def is_illegal(self):
'''
Returns True if any constraints are broken in this state, False otherwise.
'''
for node in self.constraints.involved:
for edge in self.constraints.involved[node]:
if (len(self.domains[node]) == 1) and (len(self.domains[edge]) == 1) and (self.domains[node][0] == self.domains[edge][0]):
return True
return False
def __lt__(self, other):
'''
Less than comparison method. Compares on f-value primarily, h value
if f are equal.
'''
if self.f == other.f:
return self.h < other.h
return self.f < other.f
def __eq__(self, other):
'''
Equality comparison method. Compares on the equality of domains.
'''
return self.domains == other.domains
def __str__(self):
'''
Print method for this object, returns its domains.
'''
return str(self.domains)