Ai_for_robotics/Motion Planning/first_search.py

# ----------
# User Instructions:
#
# Define a function, search() that returns a list
# in the form of [optimal path length, row, col]. For
# the grid shown below, your function should output
# [11, 4, 5].
#
# If there is no valid path from the start point
# to the goal, your function should return the string
# 'fail'
# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space

from collections import deque

import numpy as np

grid = np.array([[0, 0, 1, 0, 0, 0, 0],
                 [0, 0, 1, 0, 1, 0, 1],
                 [0, 0, 1, 0, 0, 0, 0],
                 [0, 0, 1, 1, 1, 0, 0],
                 [0, 0, 0, 0, 1, 0, 1]])
init = np.array([0, 0])
# goal = np.array([len(grid)-1, len(grid[0])-1])
goal = np.array([3, 6])
cost = np.array([1.0, 1.0, 1.0, 1.0])

delta = np.array([[-1, 0],  # go up
                  [0, -1],  # go left
                  [1, 0],  # go down
                  [0, 1]])  # go right

delta_name = ['^', '<', 'v', '>']


def check_navigable(goal) -> bool:
    if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
        # print("G: ",goal)
        if not grid[goal[0], goal[1]]:
            return True
    return False


def search(grid: np.ndarray, init: list, goal: list, cost: list):
    # ----------------------------------------
    # insert code here
    # ----------------------------------------
    path = list()
    next_check = deque()
    next_check.append(init)
    already_checked = dict()
    already_checked[tuple(init)] = 0
    cost_map = np.zeros([len(grid), len(grid[0])])
    extend_map = np.zeros([len(grid), len(grid[0])]) - 1
    extend_now = 0
    extend_map[0,0] = extend_now

    while next_check:
        checking = next_check.popleft()
        if check_navigable(checking):
            for move in delta:
                next = checking+move  # type: np.ndarray
                if check_navigable(next) and tuple(next) not in already_checked:
                    extend_now += 1
                    next_check.append(next)
                    cost_now = already_checked[checking[0], checking[1]]+1
                    already_checked[tuple(next)] = cost_now
                    cost_map[next[0], next[1]] = cost_now
                    extend_map[next[0], next[1]] = extend_now
                    if all(next == goal):
                        print(cost_map)
                        print(extend_map)
                        return [cost_now, next[0], next[1]]

    print(cost_map)
    print(extend_map)
    return "fail"


print(search(grid, init, goal, cost))