class PathFinder():
def __init__(self, start, end, clearance, rpm1, rpm2):
self.start = (start[1], start[0], start[2])
self.end = (end[1], end[0])
self.rpm1 = rpm1
self.rpm2 = rpm2
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet(self.start)
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(102, 102, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
self.output = []
def plot_arrow(self, node, parentnode, color):
Xi, Yi = parentnode[1], parentnode[0]
UL, UR = node[3], node[4] #node[1], node[0]
Thetai = parentnode[2]
t = 0
r = 0.15
L = 3.54
dt = 0.1
Xn = Xi
Yn = Yi
Thetan = 3.14 * Thetai / 180
while t < 1:
t = t + dt
Xs = Xn
Ys = Yn
Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt
Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt
Thetan += (r / L) * (UR - UL) * dt
plt.plot([Xs, Xn], [Ys, Yn], color=color)
Thetan = 180 * (Thetan) / 3.14
# print(Xn, Yn, Thetan)
return Xn, Yn, Thetan
def initialCheck(self):
if not self.obss.checkFeasibility(self.start):
print(
"Start node is in obstacle field. Please provide correct starting position."
)
return False
elif not self.obss.checkFeasibility(self.end):
print(
"Goal node is in obstacle field. Please provide correct goal position."
)
return False
else:
return True
def callforaction(self, Xi, Yi, Thetai, UL, UR):
t = 0
r = 0.38
L = 3.54
dt = 0.1
Xn = Xi
Yn = Yi
Thetan = 3.14 * Thetai / 180
while t < 1:
t = t + dt
Xs = Xn
Ys = Yn
Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt
Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt
Thetan += (r / L) * (UR - UL) * dt
# print(Thetan, "first")
# plt.plot([Xs, Xn], [Ys, Yn], color="blue")
Thetan = (180 * (Thetan) / 3.14)
# print(Thetan, "second")
# print(Xn, Yn, Thetan)
return Xn, Yn, Thetan, UL, UR
def actionSet(self, node):
x1, y1, th1, ac11, ac12 = self.callforaction(node[0], node[1], node[2],
0, self.rpm1)
x2, y2, th2, ac21, ac22 = self.callforaction(node[0], node[1], node[2],
self.rpm1, 0)
x3, y3, th3, ac31, ac32 = self.callforaction(node[0], node[1], node[2],
self.rpm1, self.rpm1)
x4, y4, th4, ac41, ac42 = self.callforaction(node[0], node[1], node[2],
0, self.rpm2)
x5, y5, th5, ac51, ac52 = self.callforaction(node[0], node[1], node[2],
self.rpm2, 0)
x6, y6, th6, ac61, ac62 = self.callforaction(node[0], node[1], node[2],
self.rpm2, self.rpm2)
x7, y7, th7, ac71, ac72 = self.callforaction(node[0], node[1], node[2],
self.rpm1, self.rpm2)
x8, y8, th8, ac81, ac82 = self.callforaction(node[0], node[1], node[2],
self.rpm2, self.rpm1)
self.actionset = (
[round(x1, 0), round(y1, 0), th1, ac11, ac12], # straight
[round(x2, 0), round(y2, 0), th2, ac21, ac22], # moveup1
[round(x3, 0), round(y3, 0), th3, ac31, ac32], # moveup2
[round(x4, 0), round(y4, 0), th4, ac41, ac42], # movedown1
[round(x5, 0), round(y5, 0), th5, ac51, ac52], # movedown2
[round(x6, 0), round(y6, 0), th6, ac61, ac62],
[round(x7, 0), round(y7, 0), th7, ac71, ac72],
[round(x8, 0), round(y8, 0), th8, ac81, ac82])
# print("actionset", self.actionset)
pass
def checkEnd(self, currentNode):
return (((currentNode[0] - self.end[0])**2) +
((currentNode[1] - self.end[1])**2)) <= 2.25
def findNewPose(self, nodeState, action):
tmp = nodeState[2]
tmp = (tmp[0] + action[0], tmp[1] + action[1])
return tmp
def dijNewPose(self, node, action):
tmp = (node[0] + action[0], node[1] + action[1], node[2] + action[2])
return tmp
def viewer(self, num):
self.showCounter += 1
if self.showCounter % num == 0:
#cv2.namedWindow("Solver", cv2.WINDOW_NORMAL)
#cv2.resizeWindow("Solver", 600, 600)
#cv2.imshow("Solver", self.obss.animationImage)
# plt.show(self.obss.obstaclespace)
if cv2.waitKey(1) & 0xFF == ord('q'):
self.view = False
pass
def trackBack(self, node, ax):
path = [self.end]
child = tuple(node)
tmp = self.end
# print("child", tuple(child), self.start)
count = 0
while True: #child[0] != self.start[0] and child[1] != self.start[1]:
self.obss.path(tmp)
parent = self.obss.getParent(child)
self.output.append([int(parent[3]), int(parent[4])])
# print("temp",temp)
self.plot_arrow(child, parent, 'b')
# self.obss.showMap(ax)
self.x_obs = [col[0] for col in self.obss.obstaclespace]
self.y_obs = [col[1] for col in self.obss.obstaclespace]
ax.scatter(self.x_obs, self.y_obs, c='r')
filename = 'fig' + str(count) + '.png'
count += 1
plt.savefig(filename, bbox_inches='tight')
tmp = (int(child[0]), int(child[1]))
# print(tmp)
path.append(tmp)
# print('parent', parent)
child = parent
if child[0] == self.start[0] and child[1] == self.start[1]:
break
self.viewer(1)
#print(child, "child")
#print(self.start, "start")
print(self.output)
return
def DijkstraSolve(self):
fig, ax = plt.subplots()
plt.grid()
ax.set_aspect('equal')
plt.xlim(0, 102)
plt.ylim(0, 102)
plt.title('Final Output', fontsize=10)
# plt.savefig('final_output.png', bbox_inches='tight')
heappush(self.Data, (0, self.start, 0))
# self.obss.astar(start, end)
con = 0
while len(self.Data) > 0:
cost, node, cost_to_come = heappop(self.Data)
# print("cost", cost)
if self.checkEnd(node):
self.goalReach = True
print("goal reached")
self.trackBack(node, ax)
break
# return
self.actionSet(node)
for action in self.actionset:
# self.dg = math.sqrt(abs(((action[0] - end[0]) ** 2) + ((action[1] - end[1]) ** 2)))
newPose = action
act1, act2 = newPose[3], newPose[4]
# self.vect(newPose, fig, ax,newPose)
# print("newpose",newPose)
if self.obss.checkFeasibility(newPose):
dg = math.sqrt(((newPose[0] - self.end[0])**2) +
((newPose[1] - self.end[1])**2))
newCost = cost_to_come + (
(newPose[3] + newPose[4]) * 0.38 / 2)
# print("dg",self.dg)
if self.obss.checkVisited(newPose):
self.obss.addExplored(newPose)
self.plot_arrow(newPose, node, 'g')
# self.obss.showMap(ax)
self.x_obs = [
col[0] for col in self.obss.obstaclespace
]
self.y_obs = [
col[1] for col in self.obss.obstaclespace
]
ax.scatter(self.x_obs, self.y_obs, c='r')
filename = 'figs' + str(con) + '.png'
con += 1
plt.savefig(filename, bbox_inches='tight')
# print(act1, act2, "act")
self.obss.addParent(newPose, node, newCost, act1, act2)
heappush(self.Data, (newCost + dg, newPose, newCost))
else:
if self.obss.getCost(newPose) > newCost + dg:
# self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost, act1,
act2)
if self.view:
pass
self.viewer(30)
if self.goalReach:
self.obss.showMap(ax)
# fig.show()
# plt.draw()
# plt.show()
else:
print("Could not find goal node...Leaving..!!")
return
class PathFinder():
def __init__(self, start, end, robotRadius, clearance):
self.start = (start[1], start[0])
self.end = (end[1], end[0])
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet()
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(300, 200, robotRadius, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
def initialCheck(self):
if not self.obss.checkFeasibility(self.start):
print(
"Start node is in obstacle field. Please provide correct starting position."
)
return False
elif not self.obss.checkFeasibility(self.end):
print(
"Goal node is in obstacle field. Please provide correct goal position."
)
return False
else:
return True
def actionSet(self):
################# [ h, w, cost]
self.actionset = [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1],
[1, 1, np.sqrt(2)], [1, -1, np.sqrt(2)],
[-1, 1, np.sqrt(2)], [-1, -1, np.sqrt(2)]]
pass
def checkEnd(self, currentNode):
return self.end == currentNode
def findNewPose(self, nodeState, action):
tmp = nodeState[2]
tmp = (tmp[0] + action[0], tmp[1] + action[1])
return tmp
def dijNewPose(self, node, action):
tmp = (node[0] + action[0], node[1] + action[1])
return tmp
def viewer(self, num):
self.showCounter += 1
if self.showCounter % num == 0:
cv2.imshow("Solver", self.obss.animationImage)
if cv2.waitKey(1) & 0xFF == ord('q'):
self.view = False
pass
def trackBack(self):
path = [self.end]
tmp = self.end
while tmp != self.start:
self.obss.path(tmp)
temp = self.obss.getParent(tmp)
tmp = (int(temp[0]), int(temp[1]))
# print(tmp)
path.append(tmp)
self.viewer(1)
return
def DijkstarSolve(self):
heappush(self.Data, (0, self.start))
while len(self.Data) > 0:
cost, node = heappop(self.Data)
if self.checkEnd(node):
self.goalReach = True
print("goal reached")
self.trackBack()
return
for action in self.actionset:
newPose = self.dijNewPose(node, action)
if self.obss.checkFeasibility(newPose):
newCost = cost + action[2]
if self.obss.checkVisited(newPose):
self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost)
heappush(self.Data, (newCost, newPose))
else:
if self.obss.getCost(newPose) > newCost:
# self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost)
if self.view:
self.viewer(30)
print("Could not find goal node...Leaving..!!")
return