Code Explaination : Revised A* algorithm with wall avoiding

Path planning algorithm(A*) for Indoor autonomous driving Robot

This path planning code is reconstructed for indoor autonomous driving robot based on original A* algorithm.
(In array form, 0 means walkable area and 1 means obstacles.)

Distance cost

Basically, cost calculation in A* algorithm is composed with summation of heuristic cost(h) and goal cost(g).
However, using this algorithm directly for robot actuation, the path is generated too close to obstacles.
Therefore, in this revision, "Distance Cost" are added to cost calculation of A* algorithm.

According to the distance of current node and nearing obstacle, the distance cost is given.
The Fast marching method was used for this approach; especially march function. (pyfmm.march)

You can check detailed explanation about Fast marching method in https://github.com/vegardkv/pyfmm

In this code, if the obstacle is nearing very next to the current node, this node will have high distance cost.
A* algorithm select the path with lowest cost, so the generated path with distance cost will be far from the obstacle.
You can adjust the weight of distance cost manually, because the value in this code is heuristic cost.

Without distance cost	With distance cost

Bug fix from original code

In original code from https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2, there were some bugs that should be fixed for the practical use.

First: Infinite loop in particular case

In particular case, especially in the map with huge size, the path calculating goes to infinite loop.
This problem was because the original code was not checking whether the neighbors were already in closed list or not.

        # Avoid infinite loop by checking closed list
        if Node(current_node, node_position) in closed_list:
            continue

Therefore, above code was inserted in "Generate children" section.

Second: Duplicated Calculation

For this problem, break was used in "Loop through children" section, not to make duplicated node which is already in closed list.
The break make the algorithm jump to main FOR loop when the child is in the closed list or child has better g cost than open node.
This revision made the algorithm run much faster than before.
(Also, in calculation of g cost and h cost, the scale of them was matched for more reasonable result.)

    # Loop through children
    for child in children:
        # Child is on the closed list
        for closed_child in closed_list:
            if child == closed_child:
                break
        else:
            # Create the f, g, and h values
            child.g = (current_node.g + ((child.position[0]-current_node.position[0])**2+(child.position[1]-current_node.position[1])**2))
            child.h = (((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2))
            # New cost 'distance cost' as dc
            child.dc = discost(maze, child.position[0], child.position[1])
            child.f = child.g + child.h + child.dc

        # Child is already in the open list
        for open_node in open_list:
            if child == open_node and child.g >= open_node.g:
                break
        else:
            # Add the child to the open list
            open_list.append(child)

In above example map, calculation time of original code was about 6.8 second.
But the revised code took about 0.34 second for calculation, which is much less than original code.

Original Code	Revised Code

Original python A* algorithm code is retrieved from

https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2