Implement A* Path Planning for a 2D Grid

1. 1
   Define the Environment and Data Structures

   Represent the robot's world as a 2D grid using NumPy. '0' denotes free space and '1' denotes obstacles. A* Path Planning requires a priority queue (open list) to efficiently select the next node to visit based on cost.
2. 2
   Define the Heuristic Function

   The heuristic function estimates the cost from the current node to the goal. For grid-based Path Planning, the Manhattan distance (`|x1-x2| + |y1-y2|`) is a common and effective choice that ensures A* finds an optimal path.
3. 3
   Implement the A* Search Algorithm

   The core of A* Path Planning. It iteratively expands nodes with the lowest 'f-cost' (g-cost + h-cost). 'g-cost' is the known cost from the start, and 'h-cost' is the heuristic estimate to the goal. This loop continues until the goal is found or there are no more nodes to explore.
4. 4
   Reconstruct and Display the Path

   After the A* search successfully finds the goal, trace back from the goal to the start using the 'came_from' dictionary. This creates the final sequence of coordinates that constitutes the optimal path.

import numpy as np
import heapq
import matplotlib.pyplot as plt

def heuristic(a, b):
    """Calculates the Manhattan distance between two points (nodes)."""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star_search(grid, start, goal):
    """Performs A* Path Planning on a grid."""
    neighbors_options = [(0, 1), (0, -1), (1, 0), (-1, 0)] # 4-directional
    close_set = set()
    came_from = {}
    gscore = {start: 0}
    fscore = {start: heuristic(start, goal)}
    oheap = []

    heapq.heappush(oheap, (fscore[start], start))
    
    while oheap:
        current = heapq.heappop(oheap)[1]

        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1]

        close_set.add(current)
        for i, j in neighbors_options:
            neighbor = current[0] + i, current[1] + j            
            if not (0 <= neighbor[0] < grid.shape[0] and 0 <= neighbor[1] < grid.shape[1]):
                continue
            if grid[neighbor[0]][neighbor[1]] == 1:
                continue
            
            tentative_g_score = gscore[current] + 1
            
            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
                continue
                
            if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1] for i in oheap]:
                came_from[neighbor] = current
                gscore[neighbor] = tentative_g_score
                fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)
                heapq.heappush(oheap, (fscore[neighbor], neighbor))
                
    return False # No path found

# --- Main execution ---
if __name__ == '__main__':
    # Define the grid: 0 = free space, 1 = obstacle
    grid = np.array([
        [0, 0, 0, 0, 1, 0, 0, 0],
        [0, 1, 0, 0, 1, 0, 1, 0],
        [0, 1, 0, 0, 0, 0, 1, 0],
        [0, 0, 0, 1, 1, 0, 1, 0],
        [0, 1, 0, 0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0, 0, 0]
    ])

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

    path = a_star_search(grid, start, goal)

    if path:
        print(f"Path found from {start} to {goal}:")
        print(path)

        # Visualization
        path_grid = np.copy(grid).astype(float)
        for p in path:
            path_grid[p] = 0.5 # Mark path with a different value
        path_grid[start] = 0.2 # Mark start
        path_grid[goal] = 0.8 # Mark goal

        plt.imshow(path_grid, cmap='viridis', interpolation='nearest')
        plt.title('A* Path Planning Result')
        plt.xticks(np.arange(grid.shape[1]))
        plt.yticks(np.arange(grid.shape[0]))
        plt.grid(True, which='both', color='white', linewidth=0.5)
        plt.show()
    else:
        print(f"No path found from {start} to {goal}.")