I'm working on this A_star algorithm in Python and when the algorithm avoids the obstacles, I need to create a safe_zone around the obstacles or the robot itself, until when the robot passes near to the obstacle, there will be no chance to hit the obstacle. So, how could I add this safe_zone? Any assistance, please?
My code is shown below:
from __future__ import print_function
import random
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],#0 are free path whereas 1's are obstacles
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 1, 0],
[0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1] #all coordinates are given in format [y,x]
cost = 1
#the cost map which pushes the path closer to the goal
heuristic = [[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])):
heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
#the actions we can take
delta = [[-1, 0 ], # go up
[ 0, -1], # go left
[ 1, 0 ], # go down
[ 0, 1 ]] # go right
#function to search the path
def search(grid,init,goal,cost,heuristic):
closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]# the referrence grid
closed[init[0]][init[1]] = 1
action = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]#the action grid
x = init[0]
y = init[1]
g = 0
f = g + heuristic[init[0]][init[0]]
cell = [[f, g, x, y]]
found = False # flag that is set when search is complete
resign = False # flag set if we can't find expand
while not found and not resign:
if len(cell) == 0:
resign = True
return "FAIL"
else:
cell.sort()#to choose the least costliest action so as to move closer to the goal
cell.reverse()
next = cell.pop()
x = next[2]
y = next[3]
g = next[1]
f = next[0]
if x == goal[0] and y == goal[1]:
found = True
else:
for i in range(len(delta)):#to try out different valid actions
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
if closed[x2][y2] == 0 and grid[x2][y2] == 0:
g2 = g + cost
f2 = g2 + heuristic[x2][y2]
cell.append([f2, g2, x2, y2])
closed[x2][y2] = 1
action[x2][y2] = i
invpath = []
x = goal[0]
y = goal[1]
invpath.append([x, y])#we get the reverse path from here
while x != init[0] or y != init[1]:
x2 = x - delta[action[x][y]][0]
y2 = y - delta[action[x][y]][1]
x = x2
y = y2
invpath.append([x, y])
path = []
for i in range(len(invpath)):
path.append(invpath[len(invpath) - 1 - i])
print("ACTION MAP")
for i in range(len(action)):
print(action[i])
return path
a = search(grid,init,goal,cost,heuristic)
for i in range(len(a)):
print(a[i])
The typical way to achieve this is to inflate the obstacles before doing the search.
Suppose your robot is circular with a radius of 25 cm. If the robot center is less than 25 cm from the obstacle, the edge of the robot will hit the obstacle, right? Thus you enlarge (inflate) the obstacle by 25 cm (meaning that any point closer than 25 cm from the original obstacle becomes itself an obstacle) so that you can plan the motion of the center of the robot.
If you want an additional safety margin of, for instance, 10 cm (that is, the edge of the robot further than 10 cm from the obstacle), you can inflate the obstacle by 35 cm instead of 25.
For Non-circular Robots, the obstacle should be inflated by at least half of the longest axis to be sure that there is no collision with the obstacles. For example, if Robot's shape is 50x80, the obstacles should be inflated by 80/2 = 40 to ensure that the trajectory is safe.
Also, note that the obstacle inflation method works best for circular/square shaped robots. For Rectangular Robots/Robots which have one longer axis, it can be problematic in the case when the Grid Map around the robot has narrow passages where even though the Robot could realistically pass through it, obstacle inflation can make it infeasible.
This obstacle inflation can be done programmatically using morphology operations on the map considered as an image. See for instance dilation in scikit-image.morphology.