I'm working on A star algorithm. I'm trying to build a trajectory for drone depending on A star. I have implemented my code below. I need to consider the height of obstacles and modify my equation :
F= G+H to F=H+G+E
E: represents the elevation of obstacles. We have the drone is flying in a specific altitude over a map, if the obstacle was very high(it means its risk very high) the distance between the obstacle and the drone is too close, so the drone will prefer to fly over the short obstacle. If the obstacle higher than the altitude of the drone, it will turn around.
I added an elevation map with random height generation and drone_altitude, but it doesn't work with me. Could I get some assistance, please?.
The A-star Python Code:
import numpy
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]
goal = [len(grid)-1,len(grid[0])-1]
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))]
elvation = numpy.random.randint(0, 100+1, size=(5, 6))
print(elvation)
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = heuristic[x][y]
e = elvation[x][y]
f = g + h + e
#our open list will contain our initial value
open = [[f, g, h, x, y]]
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()
open.reverse() #reverse the list
next = open.pop()
#print('list item')
#print('next')
x = next[3]
y = next[4]
g = next[1]
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)):
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 = elvation[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()