def start(self):
''' Start the A star algorithm '''
expansion_count = 0
while not self.open_set.empty():
current = self.open_set.get()
# print "Expanding node {0}".format(current)
expansion_count += 1
# If we reached the goal, stop
if current == self.world.goal:
break
evaluator = Agent(current, self.facing[current], self.heuristic_num, self.world)
for next in self.world.get_adjacent_cells(current):
# Tally total cost
g_score = self.cost_so_far[current] \
+ evaluator.forward(next) + evaluator.turn(next)
# Consider the adjacent node, 'next'...
if next not in self.cost_so_far or g_score < self.cost_so_far[next]:
self.cost_so_far[next] = g_score
h_score = evaluator.estimate(next, self.world.goal)
f_score = g_score + h_score
#Add the node to the priority queue
self.open_set.put(next, f_score)
#Save the direction the node is facing
new_dir = Direction().set_dir(Direction.vector(current, next))
self.facing[next] = new_dir
turn_string = "turn," * evaluator.dir.count_turns_needed(current, next)
self.actions_taken[next] = turn_string + "forward"
#Add the node to the path of traversed nodes
self.came_from[next] = current
for bash_cell in self.world.get_bashable_cells(current):
g_score = self.cost_so_far[current] \
+ evaluator.bash(bash_cell) + evaluator.turn(bash_cell)
#Consider the bash node, next
if bash_cell not in self.cost_so_far or g_score < self.cost_so_far[bash_cell]:
self.cost_so_far[bash_cell] = g_score
h_score = evaluator.estimate(bash_cell, self.world.goal)
f_score = g_score + h_score
self.open_set.put(bash_cell, f_score)
new_dir = Direction().set_dir(Direction.vector(current, bash_cell))
self.facing[bash_cell] = new_dir
turn_string = "turn," * evaluator.dir.count_turns_needed(current, bash_cell)
self.actions_taken[bash_cell] = turn_string + "bash,forward,"
self.came_from[bash_cell] = current
score = 100 - self.cost_so_far[self.world.goal]
self.trace_path()
self.output(score, expansion_count)
