Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #grid format
- # 0 = navigable space
- # 1 = occupied space
- import random
- grid = [[0, 1, 0, 0, 0, 0],
- [0, 1, 0, 0, 0, 0],
- [0, 1, 0, 0, 0, 0],
- [0, 1, 0, 0, 0, 0],
- [0, 0, 0, 0, 1, 0]]
- heuristic = [[9, 8, 7, 6, 5, 4],
- [8, 7, 6, 5, 4, 3],
- [7, 6, 5, 4, 3, 2],
- [6, 5, 4, 3, 2, 1],
- [5, 4, 3, 2, 1, 0]]
- init = [0,0] #Start location is (0,0) which we put it in open list.
- goal = [len(grid)-1,len(grid[0])-1] #Our goal in (4,5) and here are the coordinates of the cell.
- #Below the four potential actions to the single field
- delta = [[-1 , 0], #up
- [ 0 ,-1], #left
- [ 1 , 0], #down
- [ 0 , 1]] #right
- delta_name = ['^','<','V','>'] #The name of above actions
- cost = 1 #Each step costs you one
- drone_height = 60
- def search():
- #open list elements are of the type [g,x,y]
- closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
- action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
- #We initialize the starting location as checked
- closed[init[0]][init[1]] = 1
- expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
- elevation = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
- for i in range(len(grid)):
- for j in range(len(grid[0])):
- if grid[i][j] == 1:
- elevation[i][j] = random.randint(1,100)
- else:
- elevation[i][j] = 0
- # we assigned the cordinates and g value
- x = init[0]
- y = init[1]
- g = 0
- h = heuristic[x][y]
- e = elevation[x][y]
- f = g + h + e
- #our open list will contain our initial value
- open = [[f, g, h, x, y]]
- '''
- We are going to use two flags
- 1- found and it will be True when the goal position is found.
- 2- resign it will be True if we couldn't find the goal position and explore everything.
- '''
- found = False #flag that is set when search complete
- resign = False #Flag set if we can't find expand
- count = 0
- #print('initial open list:')
- #for i in range(len(open)):
- #print(' ', open[i])
- #print('----')
- while found is False and resign is False:
- #Check if we still have elements in the open list
- if len(open) == 0: #If our open list is empty, there is nothing to expand.
- resign = True
- print('Fail')
- print('############# Search terminated without success')
- print()
- else:
- #if there is still elements on our list
- #remove node from list
- open.sort() #sort elements in an increasing order from the smallest g value up
- open.reverse() #reverse the list
- next = open.pop() #remove the element with the smallest g value from the list
- #print('list item')
- #print('next')
- #Then we assign the three values to x,y and g. Which is our expantion.
- x = next[3]
- y = next[4]
- g = next[1]
- #elvation[x][y] = np.random.randint(100, size=(5,6))
- expand[x][y] = count
- count+=1
- #Check if we are done
- if x == goal[0] and y == goal[1]:
- found = True
- print(next) #The three elements above this "if".
- print('############## Search is success')
- print()
- else:
- #expand winning element and add to new open list
- for i in range(len(delta)): #going through all our actions the four actions
- #We apply the actions to x and y with additional delta to construct x2 and y2
- x2 = x + delta[i][0]
- y2 = y + delta[i][1]
- #if x2 and y2 falls into the grid
- if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:
- #if x2 and y2 not checked yet and there is not obstacles
- if closed[x2][y2] == 0 and grid[x2][y2] == 0 and e < drone_height:
- g2 = g + cost #we increment the cose
- h2 = heuristic[x2][y2]
- e2 = elevation[x2][y2]
- f2 = g2 + h2 + e2
- open.append([f2,g2,h2,x2,y2]) #we add them to our open list
- #print('append list item')
- #print([g2,x2,y2])
- #Then we check them to never expand again
- closed[x2][y2] = 1
- action[x2][y2] = i
- for i in range(len(expand)):
- print(expand[i])
- print()
- policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
- x=goal[0]
- y=goal[1]
- policy[x][y]='*'
- while x !=init[0] or y !=init[1]:
- x2=x-delta[action[x][y]][0]
- y2=y-delta[action[x][y]][1]
- policy[x2][y2]= delta_name[action[x][y]]
- x=x2
- y=y2
- for i in range(len(policy)):
- print(policy[i])
- search()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement