# A* in Python using NetworkX

###### tags: `A*`,`Python`,`Path Planning`,`NetworkX`

## Heuristic Search
* Admissible heuristic h(currentNode,goal)
- We will not accidentally miss an optimal solution.
- In the case of optimal path planning: An admissible heuristic is any "Lower Bound" on the remaining distance to the goal.
- L1-norm is NOT an admissible heuristic, because it gives an overestimation of the distance.
* Priority queue sorting order:
- A* uses 'cost-to-start + admissible heuristic'
- Dijkstra uses 'cost-to-start'

## A* Pseudocode
:::warning

Function AStarAlgorithm(G,start,goal,Q) 
UNVISITED ← V\{start}
while Q.top()≠Ø
    v ← Q.pop()
    while v.neighbor()≠Ø
    u ← v.nextneighbor()
    if u ∈ UNVISITED or u.costToStart > v.costToStart + cost(v,u)
        UNVISITED ← UNVISITED\{u}
        u.parent ← v
        u.costToStart ← v.costToStart + cost(v,u)
        Q.update(u,u.costToStart+h(u,goal))
    if v = goal
        return SUCCESS
    return FAILURE

:::

## Coding example
:::warning
Construct a nxn 8-connected Grid Graph

import networkx as nx
n     = 4
G     = nx.grid_2d_graph(n,n)

Node Initialization: Assign index from 1, set costs as zero, and label parent and 'visited' property.

idx     = 1
for node in G.nodes:
    G.nodes[node]['idx']    = idx
    G.nodes[node]['g']      = 0
    G.nodes[node]['h']      = 0
    G.nodes[node]['f']      = 0
    G.nodes[node]['parent'] = None
    G.nodes[node]['visited']= False
    if idx == start_idx:
        start_node = node
    idx = idx+1

Note that the default setting of grid graph in networkX is 4-connected, so we need to add diagonal edges manually to have graph 8-connected and set weights of the diagonal edges as 1.414 to ensure triangle inequality.

# Set all weights to 1
for edge in G.edges:
    G.edges[edge]['weight'] = 1
        
# Add diagonal edges to make 8-connected grid
G.add_edges_from([
    ((x, y), (x+1, y+1))
    for x in range(n-1)
    for y in range(n-1)
] + [
    ((x+1, y), (x, y+1))
    for x in range(n-1)
    for y in range(n-1)
], weight=1.414)

A* starts here

# Initialize priority queue and visited list
    Q       = []
    VISITED = []            # visited list
    path    = []
    Q.append(start_node)
        
if len(Q) > 0:
    current_node    = Q.pop(0)
    G.nodes[current_node]['visited'] = True
    VISITED.append(current_node)
    if current_node == goal_node:
        print('Goal!')
        path        = backTrace(G, VISITED)
        reachGoal   = True
    neighbors       = G.neighbors(current_node)
    for neighbor in neighbors:
        if neighbor not in obs_list:
            current_g   = G.nodes[neighbor]['g']
            new_g       = G.nodes[current_node]['g']+ G.get_edge_data(current_node,neighbor)['weight']
            if not G.nodes[neighbor]['visited'] or current_g > new_g:
                g           = new_g
                y_dis2goal  = int(neighbor[1])-int(goal_pos[0])
                x_dis2goal  = int(neighbor[0])-int(goal_pos[1])
                h           = np.linalg.norm((x_dis2goal,y_dis2goal), 2)
                f           = h+g
                # Update node data and priority queue
                G.nodes[neighbor]['visited'] = True
                G.nodes[neighbor]['parent'] = current_node
                G.nodes[neighbor]['g'] = g
                G.nodes[neighbor]['f'] = f
                G.nodes[neighbor]['h'] = h
                Q.append(neighbor)
                Q   = sorted(Q, key=lambda n: G.nodes[n]['f'])

:::
* [Create a 2D Grid Graph](https://networkx.org/documentation/stable/reference/generated/networkx.generators.lattice.grid_2d_graph.html)
* [8-connected grid](https://stackoverflow.com/questions/55772715/how-to-create-8-cell-adjacency-map-for-a-diagonal-enabled-a-algorithm-with-the)

## References... TBU
* [Set node color](https://stackoverflow.com/questions/27030473/how-to-set-colors-for-nodes-in-networkx)
* [Find the edge between two nodes](https://stackoverflow.com/questions/35281763/how-to-get-the-data-for-the-edge-between-two-nodes)
* [Sort nodes based on a node attribute](https://stackoverflow.com/questions/69173807/sort-nodes-of-a-networkx-graph-based-on-a-node-attribute)