def reRouteTotal(state, start, end, n, rz=15):
state.mode = "TotalReroute"
V = SampleFreeN(n, state.obstacles, state.size)
state.vis.target = (int(end[0]), int(end[1]))
print("Total ReRoute")
#V = sampleRadius(40,(0,0),state)
VOpen = BinHeap()
state.vis.nodes = V
z = Node(start[0], start[1])
state.root = z
state.vis.root = state.root
V.append(z)
z.visited = True
VOpen.insert(z)
r_z = rz
N_z = MemoNear(V, z, r_z, state)
count = 0
while (distToPoint(z, end) > state.acc):
count += 1
#need to write get Closed
vOpenNew = []
xNear = getVisited(N_z, visited=False)
for x in xNear:
N_x = MemoNear(V, x, r_z, state)
yNear = getOpen(VOpen, N_x)
#yMin = min(yNear,key = lambda y:Cost(y)+CostOfEdge(y,x)) #arg min line
#add check for empty to break
if (len(yNear) == 0):
break
yMin = min(yNear, key=lambda y: y.cost + CostLI(y, x, state)
) #arg min line#need to change this
if collisionFree(yMin, x, state.obstacles):
yMin.addChild(x)
x.setCost(CostLI(yMin, x, state))
# update cost here
vOpenNew.append(x)
x.visited = True
state.vis.update()
#pygame.event.get()
state.CheckEvents()
setOpen(VOpen, vOpenNew)
z.open = False
z.closed = True
#relook at closed stuff
if (VOpen.currentSize == 0):
print("failed Total")
return []
z = VOpen.delMin()
N_z = MemoNear(V, z, r_z, state)
#vis.update()
#state.mode = "hold"
return getPathToGoal(z)
def go(self, state, path_):
detectedNum = 0
print(self.preDetections)
path = list(path_)
state.mode = "other"
self.pathToGo = []
#path = list(reversed(path))
travelledPath = []
self.preparePath(path)
state.vis.pathToGo = list(self.pathToGo)
state.vis.update()
state.CheckEvents()
#stop traversing to reroute
instantCost = state.costMesh.getMeshCopy()
while len(self.pathToGo) > 0:
state.mode = "traversing"
time.sleep(0.1)
self.position = (int(round(self.pathToGo[0][0])),
int(round(self.pathToGo[0][1])))
travelledPath.append(self.position)
state.vis.robotPos = self.position
state.vis.update()
state.CheckEvents()
travelledPath.append(self.pathToGo.pop(0))
if (not self.checkDetected(state)):
detectedNum += 1
(reRoutePath, reRouteGoal,
success) = reRoute(state, self, instantCost)
if (success):
reRouteGoalindex = self.pathToGo.index(reRouteGoal)
endPath = self.pathToGo[reRouteGoalindex:]
self.pathToGo = []
self.preparePath(reRoutePath)
self.pathToGo = self.pathToGo + endPath
else:
self.pathToGo = []
self.preparePath(reRoutePath)
state.vis.pathToGo = list(self.pathToGo)
return travelledPath, detectedNum
(tPath, encouteredNum) = state.robot.go(state, path)
encoutered.append(encouteredNum)
travelledPath = travelledPath + tPath
state.vis.travelledPath = travelledPath
state.setCurPath([])
state.addPath(path)
state.noReroutePath = state.noReroutePath + path
plannedPathLength = measureNodePathLength(state.noReroutePath)
travelledPathLength = measurePathLength(travelledPath)
print(measureNodePathLength(state.noReroutePath))
print(measurePathLength(travelledPath))
travelled.append(travelledPathLength)
planned.append(plannedPathLength)
print(planned)
print(travelled)
print(encoutered)
state.costMesh.decay(d)
"""
plt.plot(travelled)
plt.plot(planned)
plt.show()
state.costMesh.decay(1)
"""
"""
Get statistics make cost mesh work!
"""
state.vis.root = Node(0, 0)
state.CheckEvents()
state.vis.update()
def reRoute(state, robot, instantMesh):
#find the closest node with cheepest cost
state.mode = "reRouting"
goalPoint = robot.pathToGo[-1]
successful = True
#finding the best node to traverse to
for point in robot.pathToGo:
print(instantMesh.getCost(point))
print(state.getPointCost(point))
if (instantMesh.getCost(point) == state.getPointCost(point)):
goalPoint = point
break
print(robot.position)
print(robot.pathToGo[-1])
print(goalPoint)
V = sampleRadius(distToPoint(point, robot.position, p2p=True),
robot.position, state)
VOpen = BinHeap()
state.vis.nodes = V
z = Node(robot.position[0], robot.position[1])
state.root = z
V.append(z)
z.visited = True
VOpen.insert(z)
r_z = 20
N_z = MemoNear(V, z, r_z, state)
count = 0
while (distToPoint(z, goalPoint) > 4):
count += 1
#need to write get Closed
vOpenNew = []
xNear = getVisited(N_z, visited=False)
for x in xNear:
N_x = MemoNear(V, x, r_z, state)
yNear = getOpen(VOpen, N_x)
#yMin = min(yNear,key = lambda y:Cost(y)+CostOfEdge(y,x)) #arg min line
#add check for empty to break
if (len(yNear) == 0):
break
yMin = min(yNear, key=lambda y: y.cost + CostLI(y, x, state)
) #arg min line#need to change this
if collisionFree(yMin, x, state.obstacles):
yMin.addChild(x)
x.setCost(CostLI(yMin, x, state))
# update cost here
vOpenNew.append(x)
x.visited = True
state.vis.update()
#pygame.event.get()
state.CheckEvents()
setOpen(VOpen, vOpenNew)
z.open = False
z.closed = True
#relook at closed stuff
if (VOpen.currentSize == 0):
print("failed")
totalReroute = reRouteTotal(state,
(robot.position[0], robot.position[1]),
state.target,
1500,
rz=20)
return (totalReroute, state.target, False)
z = VOpen.delMin()
N_z = MemoNear(V, z, r_z, state)
#vis.update()
state.mode = "hold"
return (getPathToGoal(z), goalPoint, True)