A* algorithm as simple as possible

I'm a beginner when it comes to path finding, so I decided to write an as simple as possible (to me) A* algorithm to learn.

I looked up on internet the general idea and found some pseudo-code that I translated in Python.

What I'm most interested in is if I'm missing some edge case or not (I wrote tests for the situations I could come up with and it works), and about optimizations.

Specifically, I don't really like going through the nodes list basically two times at every cycle.

Limitations:

• it only checks four directions instead of eight. Maybe in the future I'll use this for a game with a grid, pac-man like, movement so I excluded diagonal direction.

• The found_in_list part looks very clumsy to me, but I could not find a nicer way of skipping to the next node.

I think it could be simpler, but I couldn't find how.

from collections import defaultdict

class Node():
def __init__(self, position=None, parent=None):
if not position:
raise Exception("Position must be specified")

self.position = position
self.parent = parent

if (parent is None):
self.distance_from_start = 0
else:
self.distance_from_start = parent.distance_from_start + 1
self.distance_from_target = 0
self.cost = 0

def __eq__(self, other):
return self.position == other.position

def get_squared_distance(first, second):
squared_x_distance = (first.position - second.position) ** 2
squared_y_distance = (first.position - second.position) ** 2

return squared_x_distance + squared_y_distance

def get_path_from_node(node):
path = []
while node is not None:
path.append(node.position)
node = node.parent
return path[::-1]

def is_invalid_position(maze, node):
if (node.position >= len(maze)):
return True

if (node.position >= len(maze)):
return True

if (node.position < 0):
return True

if (node.position < 0):
return True

if (maze[node.position][node.position] != 0):
return True

return False

def astar(maze, start_position, end_position):
start_node = Node(start_position)
end_node = Node(end_position)

nodes = [start_node]
visited_nodes = defaultdict(bool)

while len(nodes) > 0:
current_index = 0
for index in range(len(nodes)):
if nodes[index].cost < nodes[current_index].cost:
current_index = index

current_node = nodes[current_index]
visited_nodes[current_node.position] = True
nodes.pop(current_index)

if current_node == end_node:
return get_path_from_node(current_node)

for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
node_position = (current_node.position + new_position, current_node.position + new_position)

new_node = Node(node_position, current_node)
if (is_invalid_position(maze, new_node)):
continue

if (visited_nodes[new_node.position]):
continue

new_node.distance_from_target = get_squared_distance(new_node, end_node)
new_node.cost = new_node.distance_from_start + new_node.distance_from_target

found_in_list = False
for node in nodes:
if new_node == node and new_node.distance_from_start > node.distance_from_start:
found_in_list = True
break

if (found_in_list):
continue
nodes.append(new_node)
return []

EDIT: an example usage

def main():
maze = [[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]

start = (0, 0)
end = (7, 6)

path = astar(maze, start, end)
print(path)

if __name__ == '__main__':
main()
• return []? Is this finished and tested? Apr 24 '19 at 11:05
• If there is no path the while len(nodes) > 0: will end, and return an empty path. If there is a path, it will return get_path_from_node(current_node). You could say it's better to return None in that case instead of [] but other than that I don't see the problem? Apr 24 '19 at 11:25
• I overlooked the early return. My bad. Apr 24 '19 at 11:29

def is_invalid_position(maze, node):

Why node? The only things used by this method are maze and node.position, so to me it would make more sense to take those as parameters.

found_in_list = False
for node in nodes:
if new_node == node and new_node.distance_from_start > node.distance_from_start:
found_in_list = True
break

if (found_in_list):
continue
nodes.append(new_node)

Surely if you find a shorter route to the same position you want to replace the previous node, not just append a new one to the list?

Well, actually it's better to use dict from position to Node and then to update the existing node, for a simple reason:

new_node = Node(node_position, current_node)
if (is_invalid_position(maze, new_node)):
continue

if (visited_nodes[new_node.position]):
continue

new_node.distance_from_target = get_squared_distance(new_node, end_node)
new_node.cost = new_node.distance_from_start + new_node.distance_from_target

calculates get_squared_distance every time the node is seen. The heuristic shouldn't change, and could potentially be a lot more expensive, so it makes sense to calculate it only the first time and then to essentially cache it in the node.

while len(nodes) > 0:
...

if current_node == end_node:
return get_path_from_node(current_node)

...

return []

As evidenced by the comments on the question, a comment in the code along the lines of "No route exists" to explain the sentinel return value would help.

current_index = 0
for index in range(len(nodes)):
if nodes[index].cost < nodes[current_index].cost:
current_index = index

is horribly slow. Since the graph is unweighted you can implement a very nice heap using an array of set. You'll probably have to implement __hash__, but I expect that best practices call for doing that whenever you implement __eq__ anyway.

To me this looks too tightly composed. A* is a graph algorithm for general graphs. This implementation hard-codes a grid graph for which A* is unnecessary: you can find the shortest path by just changing one coordinate in single steps until it matches, and then changing the other in the same way.

It's also inconsistently OO. If Node is worthy of a class, surely Maze is too? I would prefer to see an implementation which works for a general graph and an implementation of a grid graph with its neighbourhood rules and heuristic.

• Thank you for your suggestions. I don't understand what you mean by you can find the shortest path by just changing one coordinate in single steps until it matches, and then changing the other in the same way. Do you mean moving horizontally until the start x is the same as the target x and doing the same with the y? But that way you wouldn't get the shortest path, and you wouldn't be able to go around obstacles, so I'm not sure why you say A* is unnecessary? Apr 24 '19 at 12:19
• Ok, I realized just now that you're right and the shortest path is moving the x and then the y because I confined the movement to just up/down/right/left. The point about obstacles still stands though Apr 24 '19 at 12:28
• Ah, maze does slightly more than I thought. Apr 24 '19 at 13:39

Instead of a visited_nodes list you can replace it with a position to node dictionary. That way you can get the old node with node = known_nodes[node_position] (if not present create new one). Then you can add a boolean seen to the node and use that to bail out the inner loop.

Keeping the list mostly sorted using a heap helps, though you still need to look for the updated nodes in the array.