findpathamazon.py

# -*- coding: utf-8 -*-
"""FindPathAmazon.ipynb

Automatically generated by Colaboratory.

Original file is located at
    https://colab.research.google.com/drive/1dA0li3LAAQGSFkl66h7Kks83Piv1eKew
"""

import numpy as np
import matplotlib.pyplot as plt

def main(startP, endP, obstacles):
    """Sets up grid to run algorithm on"""

    # Create grid with boundaries by placing obstacles
    N = np.max([endP[0] - startP[0], endP[1] - startP[1]]) + 1   # Calculate size of grid needed
    grid = np.zeros((N+4, N+4))
    grid[1:-2, [1, -2]] = 1     # Create boundaries by placing obstacles (value=1)
    grid[[1, -2], 1:-2] = 1
    
    # Offset points by (+2, +2) to fit the grid
    startP = list(np.array(startP) + 2)
    endP = list(np.array(endP) + 2)
    obstacles = (np.array(obstacles) + 2).tolist()

    # Place obstacles in grid
    for (x, y) in obstacles:
        grid[x, y] = 1

    # Run main path finding algorithm
    pathPoints = findPath(grid, startP, endP)

    # Change back to original coordinates by (-2, -2)
    pathPoints = np.array(pathPoints) - 2
    obstacles = np.argwhere(grid==1) - 2

    return pathPoints, obstacles


def findPath(grid:np.ndarray, startP:list, endP:list):
    """Breakdown of the algorithm, with the main parts exposed."""

    pathPoints = []     # Initialize list to store final path
    currP = startP      # Starting point
    pathPoints.append(currP)

    while currP != endP:     # Run until end goal is reached

        nxtP = calcNextPoint(currP, endP)    # Get ideal next point
        nxtX, nxtY = nxtP
 
        if grid[nxtX, nxtY] == 1:        # If obstacle in next point, find cluster of obstacles and go around

            freePoints = []  
            checkObs(grid, nxtP, [], freePoints)    # Calculates free paths to go around cluster of obstacles
            freePoints = removeDuplicates(freePoints)
            
            exitP = calcExitP(freePoints, endP)     # Calculates exit point of free paths

            getPathPoint(currP, exitP, freePoints, pathPoints)  # Executes path through free paths

            currP = exitP    # Update current point

        else:
            pathPoints.append(nxtP)    # If no obstacle, add to path and keep going
            currP = nxtP
    
    return pathPoints


def dist(p1, p2):
    """Distance between two grid points"""
    return np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)


def checkObs(grid, pCheck, pIgnore, freePoints):
    """
    Called recursively. 
    Finds cluster of obstacles adjacent to initial obstacle point.
    Keeps track of free paths around the cluster.
    """

    x, y = pCheck
    obsPoints = []

    for (nx, ny) in [(x, y+1), (x, y-1), (x+1, y), (x-1, y)]:  # Checks adjacent points in cross shape

        if [nx, ny] in pIgnore:    # Points that were already checked get ignored
            continue
        if grid[nx, ny] == 0:
            freePoints.append([nx, ny])    # Store free point to be added to free path
        elif grid[nx, ny] == 1: 
            obsPoints.append([nx, ny])     # Store obstacle
        else: 
            raise ValueError("Grid point must be either 0 or 1.")
    
    pIgnore.append(pCheck)     # Ignore current center point

    for obsP in obsPoints:
        checkObs(grid, obsP, pIgnore, freePoints)   # Check all obstacle points found for more adjacent obstacles
    
    # Procedure ends when there are no more adjacent obstacles to be found
    return


def getPathPoint(currP, exitP, freePoints, pathPoints):
    """
    Called recursively.
    Stores a path on pathPoints from a given list of freePoints, starting at currP and ending at pEnd
    """

    pathPoints.append(currP)   # Each call appends point to path

    if currP == exitP:     # End recursive calling when reaching exit point
        return

    for fp in freePoints:
        d = dist(currP, fp)

        if ((d==1) or (d==np.sqrt(2))) and (fp not in pathPoints):  # Next point found is either adjacent or in diagonal
            nxtP = fp
            break
    
    getPathPoint(nxtP, exitP, freePoints, pathPoints)    # Repeat procedure for next point
    return


def removeDuplicates(freePoints):
    """Removes duplicate points of list"""
    res = []
    for fp in freePoints:
        if fp not in res:
            res.append(fp)
    return res


def calcExitP(freePoints, endP):
    """Return point from freePoints that is closest to goal"""

    closest = 1000   # Big value to be replaced at first iteration
    for fp in freePoints:
        curr = dist(fp, endP)

        if curr < closest:   # Stores minimum closest thus far
            closest = curr
            bestP = fp

    return bestP


def calcNextPoint(currP, endP):
    """
    Default way of predicting next point.
    Next point is whichever point gets closer to the end goal.
    """

    res = [0, 0]
    for i, (currCoor, endCoor) in enumerate(zip(currP, endP)):   # Loop over x, y coordinates

        # Update each coordinate by comparing it with corresponding end point coordinate
        if currCoor == endCoor:
            res[i] = currCoor
        elif currCoor > endCoor:
            res[i] = currCoor - 1
        elif currCoor < endCoor:
            res[i] = currCoor + 1
        else:
            raise ValueError("Error in determining next point")

    if res != endP:
        assert res != currP, f"res={res}, currP={currP}"
        assert res != [0, 0], f"Assignemnt did not work!"
    return res

# Coordinates of points are offset by a value of 2
startP = [0, 0]
endP = [9, 9]
obstacles = [[9, 7], [8, 7], [6, 7], [6, 8], [7, 7], [7, 8], [5, 5], [5, 4], [4, 4],
              [0, 6], [1, 6], [2, 6], [3, 6], [4, 6], [6, 7], [6, 6]]

# Option to overwrite obstacles with random points
randomObstacles = False
if randomObstacles:
    noRandPoints = 20
    obstacles = np.random.randint(0, 10, (noRandPoints, 2)).tolist()
    if startP in obstacles:
        obstacles.remove(startP)
    if endP in obstacles:
        obstacles.remove(endP)

pathPoints, obstacles = main(startP, endP, obstacles)

# Output Path and plot
print("Found the following path:", pathPoints.tolist(), "\n\n")

# Plot results
plt.style.use("seaborn-poster")
fig, ax = plt.subplots(figsize=(7, 6))
ax.scatter(obstacles[:, 0], obstacles[:, 1], label="Obstacles")
ax.scatter(pathPoints[:, 0], pathPoints[:, 1], label="Path")
ax.legend(loc="lower left", bbox_to_anchor=(0, 1), ncol=2)
ax.text(startP[0], startP[1], "Start", fontsize=18, color="r")
ax.text(endP[0], endP[1], "End", fontsize=18, color="r")
plt.show()