def __init__(self, boundry, start, goal, obstacle,thread_id):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = boundry;
self.start = start; # x,y,F,g
self.goal = goal;
#self.Islands = [[9, 15], [15, 17]];
self.Islands=[];
self.obstacle = obstacle;
# Riz Objects
#pnba_isl.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle , self.Islands);
self.printfile = PrintClass(thread_id);
self.ThreadID = thread_id;
pnba_isl.L[thread_id - 1] = [goal[0], goal[1], 2000, 2000];
print "potential islands ", pnba_isl.L[thread_id - 1];
def __init__(self,
boundary,
start,
goal,
obstacles,
space_resolution,
stepFunction,
primitives=90,
primitive_bounds=[-math.pi / 4.0, math.pi / 4.0],
rounding=8):
#Abstract Variables
self.OPEN = []
self.CLOSE = []
self.Boundry = boundary
self.Start = start
self.Goal = goal
self.Obstacles = obstacles
self.Res = space_resolution
self.Path = {}
#Heuristic Functions
self.Heur = EucliHeur(self.Start, self.Goal, self.Obstacles)
#Abstract Distance Function
self.DFunc = self.Heur.getDistance
#Class Variables
#self.StateSpace = []
self.StepFunc = stepFunction
self.NumPrimitives = primitives
self.PrimBounds = primitive_bounds
self.Primitives = self.GetPrimitives(self.NumPrimitives,
self.PrimBounds)
self.Rounding = rounding
print("Primitives: {}".format(self.Primitives))
def __init__(self):
self.OPEN1 = []
self.OPEN2 = []
self.CLOSED1 = []
self.CLOSED2 = []
self.Boundry = [[0, 0], [20, 20]]
self.start = [0, 0, 0, 0]
# x,y,F,g
self.goal = [20, 20, 0, 20]
self.Islands = [[9, 15], [15, 17]]
#Identify islands here
self.obstacle = [[12, 12], [11, 13], [10, 14], [13, 11], [14, 10],
[13, 13], [12, 14], [11, 15], [14, 12], [15, 11],
[11, 11], [10, 13], [12, 10], [13, 9], [13, 12],
[12, 13], [11, 12], [12, 11]]
# Riz Objects
self.Dsp = display_2d(self.Boundry)
self.Heur = EucliHeur(self.start, self.goal, self.obstacle,
self.Islands)
def __init__(self, boundry, start, goal, obstacle, thread_id):
self.OPEN1 = []
self.OPEN2 = []
self.CLOSED1 = []
self.CLOSED2 = []
self.Boundry = boundry
self.start = start
# x,y,F,g
self.goal = goal
self.Islands = [[]]
#Islands have no use in this algorithm
self.obstacle = obstacle
# Riz Objects
#pnba.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle,
self.Islands)
self.printfile = PrintClass(thread_id)
self.ThreadID = thread_id
def __init__(self, boundry, start, goal, obstacle,thread_id):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = boundry;
self.start = start; # x,y,F,g
self.goal = goal;
self.Islands=[[]]; #Islands have no use in this algorithm
self.obstacle = obstacle;
# Riz Objects
#pnba.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle , self.Islands);
self.printfile = PrintClass(thread_id);
self.ThreadID = thread_id;
def __init__(self):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = [[0, 0], [20, 20]];
self.start = [0, 0, 0, 0]; # x,y,F,g
self.goal = [20, 20, 0, 20];
self.Islands = [[9, 15], [15,17]]; #Identify islands here
self.obstacle = [
[12, 12], [11, 13], [10, 14], [13, 11], [14, 10],
[13, 13], [12, 14], [11, 15], [14, 12], [15, 11],
[11, 11], [10, 13], [12, 10], [13, 9],
[13, 12], [12, 13], [11, 12], [12, 11]
];
# Riz Objects
self.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle, self.Islands);
class pnba_isl (Algorithm):
Dsp = [];
Heur = [];
#Static
M = [];
L = [ [0,0,0,3000], [1,1,1,3000] ];
solution_found = False;
printfile = None;
ThreadID = None;
#staticFlags
SOLUTION_FOUND = "solution_found"
GOAL_FOUND = "goal_found"
NO_SUCC = "no_succ"
SUCC = "succ"
UNDISCOVERED = "undisc"
DISCOVERED = "disc"
FALIURE = "faliure"
def __init__(self, boundry, start, goal, obstacle,thread_id):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = boundry;
self.start = start; # x,y,F,g
self.goal = goal;
#self.Islands = [[9, 15], [15, 17]];
self.Islands=[];
self.obstacle = obstacle;
# Riz Objects
#pnba_isl.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle , self.Islands);
self.printfile = PrintClass(thread_id);
self.ThreadID = thread_id;
pnba_isl.L[thread_id - 1] = [goal[0], goal[1], 2000, 2000];
print "potential islands ", pnba_isl.L[thread_id - 1];
def isDiscovered(self, n):
if(pnba_isl.M[n[0]][n[1]] == self.ThreadID):
return True;
else:
return False;
#For deciding from two lists
def getNextElement(self):
x = [];
if (len(self.OPEN1) == 0):
x = self.OPEN2.pop(0);
return 2, x;
elif (len(self.OPEN2) == 0):
x = self.OPEN1.pop(0);
return 1, x;
else:
if (self.OPEN1[0][2] > self.OPEN2[0][2]):
x = self.OPEN2.pop(0);
return 2, x;
else:
x = self.OPEN1.pop(0);
return 1, x
def getNext(self):
list_name, n = self.getNextElement();
if(pnba_isl.M[n[0]][n[1]] > 0):
if(pnba_isl.M[n[0]][n[1]] == self.ThreadID):
return pnba_isl.DISCOVERED, list_name, n
elif(self.isGoal(n)):
return pnba_isl.GOAL_FOUND, list_name, n
elif(pnba_isl.M[n[0]][n[1]] != self.ThreadID):
return pnba_isl.SOLUTION_FOUND, list_name, n
else:
return pnba_isl.FALIURE, list_name, n
elif(pnba_isl.M[n[0]][n[1]] < 0):
return pnba_isl.UNDISCOVERED, list_name, n
else:
return pnba_isl.FALIURE, list_name, n;
def updateFCost(self, n):
n[2] = self.Heur.getHeuristic(n) + self.Heur.getFixedCost();
def Expand(self, n):
expanded = []
x = [n[0] + 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0], n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0], n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba_isl.Dsp.addSubPoint(x);
if (len(expanded) == 0):
return False, expanded
else:
return True, expanded
def run(self):
MyThread.threadLock.acquire();
#pnba_isl.Dsp.addPoint(self.start);
#pnba_isl.Dsp.addGoalPoint(self.goal);
self.printfile.writeToFile(self.start);
pnba_isl.solution_found = False;
MyThread.threadLock.release();
##pnba_isl.Dsp.addIslandPoints(self.Islands);
##pnba_isl.Dsp.addObstaclePoints(self.obstacle);
# step1
self.start[2] = self.Heur.getHeuristic(self.start);
self.OPEN1.append(self.start);
self.Islands.append(self.goal);
self.Heur.Islands = self.Islands;
while(not pnba_isl.solution_found):
if (self.isOpenEmpty()):
print("OPEN is empty");
pnba_isl.solution_found = False;
break;
t, list_name, x = self.getNext();
if(t == pnba_isl.GOAL_FOUND):
print "GOAL_FOUND by: ", self.ThreadID
break;
elif(t == pnba_isl.SOLUTION_FOUND):
print "SOLUTION_FOUND by: ", self.ThreadID
MyThread.threadLock.acquire();
pnba_isl.solution_found = True;
MyThread.threadLock.release();
break;
elif(t == pnba_isl.DISCOVERED):
print "Already Discovered"
continue;
elif(t == pnba_isl.FALIURE):
print "FALIURE";
MyThread.threadLock.acquire();
self.printfile.writeToFile(x);
MyThread.threadLock.release();
if(x[2] <= pnba_isl.L[2 - self.ThreadID][3]):
MyThread.threadLock.acquire();
self.Islands = [];
self.Islands.append(pnba_isl.L[self.ThreadID-1]);
self.Heur.Islands = self.Islands;
MyThread.threadLock.release();
succ_exist, succ = self.Expand(x);
if (not succ_exist):
print("No Successor Found");
continue;
for s in succ:
#s[3] = s[3] + self.Heur.getFixedCost();
#if (not self.isInList(s, self.OPEN1) and not self.isInList(s, self.OPEN2)):
# d = self.Heur.getMinH(s) - self.Heur.getHeuristic(s);
# if (d > 0):
# s[2] = self.Heur.getMinH(s);
# self.OPEN1.append(s);
# else:
# s[2] = self.Heur.getHeuristic(s);
# self.OPEN2.append(s);
#elif (self.isInList(s, self.OPEN1)):
# temp, element = self.getFromList(s, self.OPEN1);
# self.OPEN1.pop(element);
# s[2] = self.Heur.getMinH(s);
# self.OPEN1.append(s);
#elif (self.isInList(s, self.OPEN2)):
# temp, element = self.getFromList(s, self.OPEN2);
# self.OPEN2.pop(element);
# s[2] = self.Heur.getHeuristic(s);
# self.OPEN2.append(s);
#If Island or from OPEN2
if(list_name == 2 or self.isIsland(x)):
if (self.isInList(s, self.OPEN1)):
temp, element = self.getFromList(s, self.OPEN1);
self.OPEN1.pop(element);
elif (self.isInList(s, self.OPEN2)):
temp, element = self.getFromList(s, self.OPEN2);
self.OPEN2.pop(element);
self.OPEN2.append(s);
#If not Island or from OPEN1
if(list_name == 1 or not self.isIsland(x)):
if(not self.isInList(s, self.OPEN1) and not self.isInList(s, self.OPEN2)):
d = self.Heur.getMinH(s) - self.Heur.getHeuristic(s);
if (d > 0):
s[2] = self.Heur.getMinH(s);
self.OPEN1.append(s);
else:
s[2] = self.Heur.getHeuristic(s);
self.OPEN2.append(s);
elif(self.isInList(s, self.OPEN1)):
temp, element = self.getFromList(s, self.OPEN1);
self.OPEN1.pop(element);
s[2] = self.Heur.getMinH(s);
self.OPEN1.append(s);
elif(self.isInList(s, self.OPEN2)):
temp, element = self.getFromList(s, self.OPEN2);
self.OPEN2.pop(element);
s[2] = self.Heur.getHeuristic(s);
self.OPEN2.append(s);
#---------Old code---------
#if(self.isInList(s,self.OPEN1)):
# temp, element = self.getFromList(s, self.OPEN1);
# self.OPEN1.pop(element);
#self.OPEN1.append(s);
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s);
if(h < pnba_isl.L[2-self.ThreadID][3]):
print "Locking Thread on ", self.ThreadID;
MyThread.threadLock.acquire();
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s);
if (h < pnba_isl.L[2-self.ThreadID][3]):
pnba_isl.L[2 - self.ThreadID] = x;
pnba_isl.L[2 - self.ThreadID][3] = h;
pnba_isl.L[self.ThreadID-1][3] = h;
MyThread.threadLock.release();
else:
print "Not Entering Main Algorithm"
pnba_isl.M[x[0]][x[1]] = self.ThreadID;
self.sortList(self.OPEN1);
self.sortList(self.OPEN2);
self.printfile.file.close();
class chakra(Algorithm):
Dsp = []
Heur = []
def __init__(self):
self.OPEN1 = []
self.OPEN2 = []
self.CLOSED1 = []
self.CLOSED2 = []
self.Boundry = [[0, 0], [20, 20]]
self.start = [0, 0, 0, 0]
# x,y,F,g
self.goal = [20, 20, 0, 20]
self.Islands = [[9, 15], [15, 17]]
#Identify islands here
self.obstacle = [[12, 12], [11, 13], [10, 14], [13, 11], [14, 10],
[13, 13], [12, 14], [11, 15], [14, 12], [15, 11],
[11, 11], [10, 13], [12, 10], [13, 9], [13, 12],
[12, 13], [11, 12], [12, 11]]
# Riz Objects
self.Dsp = display_2d(self.Boundry)
self.Heur = EucliHeur(self.start, self.goal, self.obstacle,
self.Islands)
def getNext(self):
x = []
if (len(self.OPEN1) == 0):
x = self.OPEN2.pop(0)
if (not self.isInList(x, self.CLOSED2)):
self.CLOSED2.append(x)
return 2, x
else:
return 0, x
elif (len(self.OPEN2) == 0):
x = self.OPEN1.pop(0)
if (not self.isInList(x, self.CLOSED1)):
self.CLOSED1.append(x)
return 1, x
else:
return 0, x
else:
if (self.OPEN1[0][2] > self.OPEN2[0][2]):
x = self.OPEN2.pop(0)
if (not self.isInList(x, self.CLOSED2)):
self.CLOSED2.append(x)
return 2, x
else:
return 0, x
else:
x = self.OPEN1.pop(0)
if (not self.isInList(x, self.CLOSED1)):
self.CLOSED1.append(x)
return 1, x
else:
return 0, x
def updateFCost(self, n):
n[2] = self.Heur.getHeuristic(n) + self.Heur.getFixedCost()
def Expand(self, n):
expanded = []
x = [n[0] + 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0], n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0] + 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0] - 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0], n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0] - 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0] + 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
x = [n[0] - 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1)
and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x)
expanded.append(x)
self.Dsp.addSubPoint(x)
if (len(expanded) == 0):
return False, expanded
else:
return True, expanded
class AStar(Algorithm):
# Assumption: state = (id,prim,x,y,r,F,g)
#Static Flags
SOLUTION_FOUND = 0
GOAL_FOUND = 1
NO_SUCC = 2
SUCC = 3
UNDISCOVERED = 4
DISCOVERED = 5
FALIURE = 6
#STATE
isSolutionFound = False
def __init__(self,
boundary,
start,
goal,
obstacles,
space_resolution,
stepFunction,
primitives=90,
primitive_bounds=[-math.pi / 4.0, math.pi / 4.0],
rounding=8):
#Abstract Variables
self.OPEN = []
self.CLOSE = []
self.Boundry = boundary
self.Start = start
self.Goal = goal
self.Obstacles = obstacles
self.Res = space_resolution
self.Path = {}
#Heuristic Functions
self.Heur = EucliHeur(self.Start, self.Goal, self.Obstacles)
#Abstract Distance Function
self.DFunc = self.Heur.getDistance
#Class Variables
#self.StateSpace = []
self.StepFunc = stepFunction
self.NumPrimitives = primitives
self.PrimBounds = primitive_bounds
self.Primitives = self.GetPrimitives(self.NumPrimitives,
self.PrimBounds)
self.Rounding = rounding
print("Primitives: {}".format(self.Primitives))
#--Overwrite Functions--
def getNext(self):
n = None
while True:
n = self.OPEN.pop(0)
if not self.isDiscivered(n) or len(self.OPEN) == 0:
break
self.CLOSE.append(self.MakePoint(n))
if self.isGoal(n):
return AStar.GOAL_FOUND, n
else:
return AStar.UNDISCOVERED, n
def updateFCost(self, n):
n[6] += self.Heur.getFixedCost()
n[5] = self.Heur.getHeuristic(n) + n[6]
return n[5], n[6]
def Expand(self, n):
succs = []
for prim in self.Primitives:
nx, ny, tn = self.StepFunc(n[2] + self.Res, n[3] + self.Res, n[4],
prim)
state = [n[0], prim, nx, ny, tn, 0, n[6]]
if self.isInLimits(state) and (not self.isDiscivered(state)):
state[5], state[6] = self.updateFCost(state)
succs.append(state)
# print("Succ: ({},{},{},{}) => ({},{},{}) H: {}".format(n[2],n[3],n[4],prim, nx,ny,tn, state[5]))
# else:
# if not self.isInLimits(state):
# print("Succs notin bound: {}".format(state))
# elif(self.isDiscivered(state)):
# print("Succs discovered: {}".format(state))
if len(succs) == 0:
return False, succs
else:
return True, succs
#--Class Functions--
def isDiscivered(self, n):
# p = self.MakePoint(n)
# print("Checking {} => {} in {}".format(n,p,self.CLOSE))
# return p in self.CLOSE
return self.isInList(n, self.CLOSE)
def MakePoint(self, n):
# return ( round(n[2],self.Rounding), round(n[3],self.Rounding), round(n[4],self.Rounding))
return n
# def StorePoint(self, n):
# self.StateSpace.append(self.MakePoint(n))
# def ScaleState(self, n):
# return round((self.DFunc(n, [0,0]))/self.Res, 4)
def GetPrimitives(self, num_of_primitives, bounds):
return np.linspace(bounds[0], bounds[1], num_of_primitives)
def Reconstruct(self):
path = []
key = self.getLastId()
while key > 0:
data = self.Path[key]
path.append(data[1])
key = data[0]
return path
def ExpansionConstruct(self):
path = []
for key in self.Path.keys():
path.append(self.Path[key][1])
return path
def run(self):
AStar.isSolutionFound = False
self.Start[5] = self.Heur.getHeuristic(self.Start)
self.Start[0] = self.getNewId()
self.Start[1] = 0.0
self.OPEN.append(self.Start)
itr = 0
while (not AStar.isSolutionFound):
print("Iterations: {}".format(itr))
itr += 1
if (not len(self.OPEN) > 0):
print("OPEN Empty")
break
elif (itr > 500):
print("Max States Expanded")
break
status, state = self.getNext()
new_id = self.getNewId()
self.Path[new_id] = [state[0], state[1]]
state[0] = new_id
if status == AStar.GOAL_FOUND:
print("Goal found!")
AStar.isSolutionFound = True
break
elif status == AStar.DISCOVERED:
print("Discovered State Found")
continue
elif status == AStar.FALIURE:
print("Faliure")
break
# print("Expanding: {} From: {}".format(state, self.OPEN))
succ_exist, succs = self.Expand(state)
if not succ_exist:
print("No Successor Found")
continue
for s in succs:
# s[6] += self.Heur.getFixedCost()
# if(self.isInList(s, self.OPEN)):
# # print("{} exists in OPEN".format(s))
# isFound, element = self.getFromList(s, self.OPEN)
# if isFound:
# self.OPEN.pop(element)
self.OPEN.append(s)
self.sortList(self.OPEN)
# print("OPEN: {}".format(self.OPEN))
if AStar.isSolutionFound:
return self.Reconstruct()
else:
return self.ExpansionConstruct()
class pnba (Algorithm):
Dsp = [];
Heur = [];
#Static
M = [];
L = 2000;
solution_found = False;
printfile = None;
ThreadID = None;
#staticFlags
SOLUTION_FOUND = "solution_found"
GOAL_FOUND = "goal_found"
NO_SUCC = "no_succ"
SUCC = "succ"
UNDISCOVERED = "undisc"
DISCOVERED = "disc"
FALIURE = "faliure"
def __init__(self, boundry, start, goal, obstacle,thread_id):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = boundry;
self.start = start; # x,y,F,g
self.goal = goal;
self.Islands=[[]]; #Islands have no use in this algorithm
self.obstacle = obstacle;
# Riz Objects
#pnba.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle , self.Islands);
self.printfile = PrintClass(thread_id);
self.ThreadID = thread_id;
def isDiscovered(self, n):
if(pnba.M[n[0]][n[1]] == self.ThreadID):
return True;
else:
return False;
def getNext(self):
n = self.OPEN1.pop(0);
if(pnba.M[n[0]][n[1]] > 0):
if(pnba.M[n[0]][n[1]] == self.ThreadID):
return pnba.DISCOVERED, n
elif(self.isGoal(n)):
return pnba.GOAL_FOUND, n
elif(pnba.M[n[0]][n[1]] != self.ThreadID):
return pnba.SOLUTION_FOUND, n
else:
return pnba.FALIURE, n
elif(pnba.M[n[0]][n[1]] < 0):
return pnba.UNDISCOVERED, n
else:
return pnba.FALIURE, n;
def updateFCost(self, n):
n[2] = self.Heur.getHeuristic(n) + self.Heur.getFixedCost();
def Expand(self, n):
expanded = []
x = [n[0] + 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0], n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0], n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x);
expanded.append(x);
#pnba.Dsp.addSubPoint(x);
if (len(expanded) == 0):
return False, expanded
else:
return True, expanded
def run(self):
MyThread.threadLock.acquire();
#pnba.Dsp.addPoint(self.start);
#pnba.Dsp.addGoalPoint(self.goal);
self.printfile.writeToFile(self.start);
pnba.solution_found = False;
MyThread.threadLock.release();
##pnba.Dsp.addIslandPoints(self.Islands);
##pnba.Dsp.addObstaclePoints(self.obstacle);
# step1
self.start[2] = self.Heur.getHeuristic(self.start);
self.OPEN1.append(self.start);
while(not pnba.solution_found):
if (self.isOpenEmpty()):
print("OPEN is empty");
pnba.solution_found = False;
break;
t, x = self.getNext();
if(t == pnba.GOAL_FOUND):
print "GOAL_FOUND by: ", self.ThreadID
break;
elif(t == pnba.SOLUTION_FOUND):
print "SOLUTION_FOUND by: ", self.ThreadID
MyThread.threadLock.acquire();
pnba.solution_found = True;
MyThread.threadLock.release();
break;
elif(t == pnba.DISCOVERED):
continue;
elif(t == pnba.FALIURE):
print "FALIURE";
MyThread.threadLock.acquire();
self.printfile.writeToFile(x);
MyThread.threadLock.release();
if(x[2] <= pnba.L):
succ_exist, succ = self.Expand(x);
if (not succ_exist):
print("No Successor Found");
continue;
for s in succ:
s[3] = s[3] + self.Heur.getFixedCost();
if(self.isInList(s,self.OPEN1)):
temp, element = self.getFromList(s, self.OPEN1);
self.OPEN1.pop(element);
self.OPEN1.append(s);
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s);
if(h < pnba.L):
MyThread.threadLock.acquire();
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s);
if (h < pnba.L):
pnba.L = h;
MyThread.threadLock.release();
pnba.M[x[0]][x[1]] = self.ThreadID;
self.sortList(self.OPEN1);
self.printfile.file.close();
class pnba(Algorithm):
Dsp = []
Heur = []
#Static
M = []
L = 2000
solution_found = False
printfile = None
ThreadID = None
#staticFlags
SOLUTION_FOUND = "solution_found"
GOAL_FOUND = "goal_found"
NO_SUCC = "no_succ"
SUCC = "succ"
UNDISCOVERED = "undisc"
DISCOVERED = "disc"
FALIURE = "faliure"
def __init__(self, boundry, start, goal, obstacle, thread_id):
self.OPEN1 = []
self.OPEN2 = []
self.CLOSED1 = []
self.CLOSED2 = []
self.Boundry = boundry
self.start = start
# x,y,F,g
self.goal = goal
self.Islands = [[]]
#Islands have no use in this algorithm
self.obstacle = obstacle
# Riz Objects
#pnba.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle,
self.Islands)
self.printfile = PrintClass(thread_id)
self.ThreadID = thread_id
def isDiscovered(self, n):
if (pnba.M[n[0]][n[1]] == self.ThreadID):
return True
else:
return False
def getNext(self):
n = self.OPEN1.pop(0)
if (pnba.M[n[0]][n[1]] > 0):
if (pnba.M[n[0]][n[1]] == self.ThreadID):
return pnba.DISCOVERED, n
elif (self.isGoal(n)):
return pnba.GOAL_FOUND, n
elif (pnba.M[n[0]][n[1]] != self.ThreadID):
return pnba.SOLUTION_FOUND, n
else:
return pnba.FALIURE, n
elif (pnba.M[n[0]][n[1]] < 0):
return pnba.UNDISCOVERED, n
else:
return pnba.FALIURE, n
def updateFCost(self, n):
n[2] = self.Heur.getHeuristic(n) + self.Heur.getFixedCost()
def Expand(self, n):
expanded = []
x = [n[0] + 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0], n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0], n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isDiscovered(x)):
self.updateFCost(x)
expanded.append(x)
#pnba.Dsp.addSubPoint(x);
if (len(expanded) == 0):
return False, expanded
else:
return True, expanded
def run(self):
MyThread.threadLock.acquire()
#pnba.Dsp.addPoint(self.start);
#pnba.Dsp.addGoalPoint(self.goal);
self.printfile.writeToFile(self.start)
pnba.solution_found = False
MyThread.threadLock.release()
##pnba.Dsp.addIslandPoints(self.Islands);
##pnba.Dsp.addObstaclePoints(self.obstacle);
# step1
self.start[2] = self.Heur.getHeuristic(self.start)
self.OPEN1.append(self.start)
while (not pnba.solution_found):
if (self.isOpenEmpty()):
print("OPEN is empty")
pnba.solution_found = False
break
t, x = self.getNext()
if (t == pnba.GOAL_FOUND):
print "GOAL_FOUND by: ", self.ThreadID
break
elif (t == pnba.SOLUTION_FOUND):
print "SOLUTION_FOUND by: ", self.ThreadID
MyThread.threadLock.acquire()
pnba.solution_found = True
MyThread.threadLock.release()
break
elif (t == pnba.DISCOVERED):
continue
elif (t == pnba.FALIURE):
print "FALIURE"
MyThread.threadLock.acquire()
self.printfile.writeToFile(x)
MyThread.threadLock.release()
if (x[2] <= pnba.L):
succ_exist, succ = self.Expand(x)
if (not succ_exist):
print("No Successor Found")
continue
for s in succ:
s[3] = s[3] + self.Heur.getFixedCost()
if (self.isInList(s, self.OPEN1)):
temp, element = self.getFromList(s, self.OPEN1)
self.OPEN1.pop(element)
self.OPEN1.append(s)
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s)
if (h < pnba.L):
MyThread.threadLock.acquire()
h = self.Heur.getHeuristic(s) + self.Heur.getCost(s)
if (h < pnba.L):
pnba.L = h
MyThread.threadLock.release()
pnba.M[x[0]][x[1]] = self.ThreadID
self.sortList(self.OPEN1)
self.printfile.file.close()
class chakra (Algorithm):
Dsp = [];
Heur = [];
def __init__(self):
self.OPEN1 = [];
self.OPEN2 = [];
self.CLOSED1 = [];
self.CLOSED2 = [];
self.Boundry = [[0, 0], [20, 20]];
self.start = [0, 0, 0, 0]; # x,y,F,g
self.goal = [20, 20, 0, 20];
self.Islands = [[9, 15], [15,17]]; #Identify islands here
self.obstacle = [
[12, 12], [11, 13], [10, 14], [13, 11], [14, 10],
[13, 13], [12, 14], [11, 15], [14, 12], [15, 11],
[11, 11], [10, 13], [12, 10], [13, 9],
[13, 12], [12, 13], [11, 12], [12, 11]
];
# Riz Objects
self.Dsp = display_2d(self.Boundry);
self.Heur = EucliHeur(self.start, self.goal, self.obstacle, self.Islands);
def getNext(self):
x = [];
if (len(self.OPEN1) == 0):
x = self.OPEN2.pop(0);
if (not self.isInList(x, self.CLOSED2)):
self.CLOSED2.append(x);
return 2, x;
else:
return 0, x;
elif (len(self.OPEN2) == 0):
x = self.OPEN1.pop(0);
if (not self.isInList(x, self.CLOSED1)):
self.CLOSED1.append(x);
return 1, x;
else:
return 0, x;
else:
if (self.OPEN1[0][2] > self.OPEN2[0][2]):
x = self.OPEN2.pop(0);
if (not self.isInList(x, self.CLOSED2)):
self.CLOSED2.append(x);
return 2, x;
else:
return 0, x;
else:
x = self.OPEN1.pop(0);
if (not self.isInList(x, self.CLOSED1)):
self.CLOSED1.append(x);
return 1, x;
else:
return 0, x;
def updateFCost(self, n):
n[2] = self.Heur.getHeuristic(n) + self.Heur.getFixedCost();
def Expand(self, n):
expanded = []
x = [n[0] + 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0], n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1], 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0], n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0] + 1, n[1] - 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
x = [n[0] - 1, n[1] + 1, 0, 1000]
if (self.isInLimits(x) and not self.isInList(x, self.CLOSED1) and not self.isInList(x, self.CLOSED2)):
self.updateFCost(x);
expanded.append(x);
self.Dsp.addSubPoint(x);
if (len(expanded) == 0):
return False, expanded
else:
return True, expanded
start = [0, 0, 0, 0]
#x,y,F,g
goal = [20, 20, 0, 20]
isl = [[9, 15]]
obstacle = [[12, 12], [11, 13], [10, 14], [13, 11], [14, 10], [13,
13], [12, 14],
[11, 15], [14, 12], [15, 11], [11, 11], [10, 13], [12, 10],
[13, 9], [13, 12], [12, 13], [11, 12], [12, 11]]
#Riz Objects
Dsp = display_2d(Boundry)
Bot = Robot(Boundry, start)
Heur = EucliHeur(start, goal, obstacle, isl)
def isOpenEmpty():
if (len(OPEN1) == 0 and len(OPEN2) == 0):
return True
else:
return False
def isGoal(n):
if (n[0] == goal[0] and n[1] == goal[1]):
return True
else:
return False