# img = eroded.copy()
# zeros = size - cv2.countNonZero(img)
# if zeros==size:
# done = True
#cv2.imshow("skel",skel)
#D* Lite Method ---------------------------------------------
newHeight = int(height * 0.1)
newWidth = int(width * 0.1)
dliteimage = cv2.resize(agvcmap.getImage(), (newWidth, newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
robot = Robot(TEG.x, TEG.y, TEG.radius * 2)
imageMap = ImageReader()
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap, robot)
moveGrid = imageMap.convertToGrid().copy()
##goal = point(3,17)
testdivider = 1
goal = point(int(newHeight / testdivider * 0.8),
int(newWidth / testdivider * 0.8))
#cv2.waitKey(0)
##mapper.printMoveGrid()
print "STARTIN LOOP"
moveId = 0
Xlength = mapper.grid.shape[0] / testdivider
Ylength = mapper.grid.shape[1] / testdivider
#dstar = dstar3.DStar(Xlength,Ylength,goal)
# zeros = size - cv2.countNonZero(img)
# if zeros==size:
# done = True
#cv2.imshow("skel",skel)
#D* Lite Method ---------------------------------------------
newHeight = int(height*0.1)
newWidth = int(width*0.1)
dliteimage = cv2.resize(agvcmap.getImage(),(newWidth,newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
robot = Robot(TEG.x,TEG.y,TEG.radius*2)
imageMap = ImageReader()
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap,robot)
moveGrid = imageMap.convertToGrid().copy()
##goal = point(3,17)
testdivider = 1
goal = point(int(newHeight/testdivider*0.8),int(newWidth/testdivider*0.8))
#cv2.waitKey(0)
##mapper.printMoveGrid()
print "STARTIN LOOP"
moveId=0
Xlength = mapper.grid.shape[0]/testdivider
Ylength = mapper.grid.shape[1]/testdivider
#dstar = dstar3.DStar(Xlength,Ylength,goal)
dstar = dlite.Dlite(Xlength,Ylength,goal,robot)
def calculatePath(self):
height = self.map.height
width = self.map.width
newHeight = int(height*0.1)
newWidth = int(width*0.1)
dliteimage = cv2.resize(self.map.getImage(),(newWidth,newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
robot = Robot(self.vehicle.x,self.vehicle.y,self.vehicle.radius*2)
imageMap = ImageReader()
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap,robot)
moveGrid = imageMap.convertToGrid().copy()
##goal = point(3,17)
testdivider = 1
self.goal = point(int(newHeight/testdivider*0.8),int(newWidth/testdivider*0.8))
#cv2.waitKey(0)
##mapper.printMoveGrid()
print "STARTIN LOOP"
moveId=0
Xlength = mapper.grid.shape[0]/testdivider
Ylength = mapper.grid.shape[1]/testdivider
#dstar = dstar3.DStar(Xlength,Ylength,goal)
dstar = dlite.Dlite(Xlength,Ylength,self.goal,robot)
print "Entering Loop"
testvar = 0
#for i in range(10):
while (robot.y != self.goal.y or robot.x != self.goal.x) :
if testvar%2 == 0:
newObs = obstacle.Obstacle(random.randint(0,height),random.randint(0,width), 40)
self.map.placeObstacle(newObs,3)
self.obsList.append(newObs)
#Place obstacles on map
self.map.updateObstacles(self.obsList)
#Morph the obstacles
self.map.updateMorph();
dliteimage = cv2.resize(self.map.getImage(),(newWidth,newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap,robot)
moveGrid = imageMap.convertToGrid().copy()
testvar = testvar + 1
moveId = moveId+1
print moveId
if path.pathIsBroken(mapper.grid) :
path.restart()
print "The path is broken"
# dstar2.dstar(mapper, robot, goal, path)
dstar.dstar(mapper, robot, self.goal, path)
#dlite.dstar(robot,goal,path)
# # DstarLite.dstar(mapper, robot, goal, path)
# astar.astar(mapper, robot, goal, path)
pathNode=path.getNextMove()
robot.x = pathNode.x
robot.y = pathNode.y
mapper.moveGrid[pathNode.x][pathNode.y]="1"
#mapper.printMoveGrid()
self.vehicle.x = pathNode.x
self.vehicle.y = pathNode.y
self.vehicle.addPosition(pathNode.x,pathNode.y)
mapper.updateMap(robot)
#raw_input("TEST")
cv2.imshow('AGVC Map', self.map.getMap())
cv2.imshow('AGVC Map Morphed', self.map.getImage())
for i in range(len(self.vehicle.positions)):
self.map.placeRobot(self.vehicle.positions[i][1]*height/newHeight,self.vehicle.positions[i][0]*width/newWidth,self.vehicle.radius)
nPoints = len(self.vehicle.positions)
points = np.array(self.vehicle.positions)*height/newHeight
#points = np.random.rand(nPoints,2)*200
xpoints = [p[0] for p in points]
ypoints = [p[1] for p in points]
xvals, yvals = self.bezier_curve(points, nTimes=1000)
for i in range(len(xvals)):
self.map.placeRobot(int(yvals[i]),int(xvals[i]),self.vehicle.radius*2)
self.map.updateActObstacles(self.obsList)
cv2.waitKey(0)
def calculatePath(self):
height = self.map.height
width = self.map.width
newHeight = int(height * 0.1)
newWidth = int(width * 0.1)
dliteimage = cv2.resize(self.map.getImage(), (newWidth, newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
robot = Robot(self.vehicle.x, self.vehicle.y, self.vehicle.radius * 2)
imageMap = ImageReader()
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap, robot)
moveGrid = imageMap.convertToGrid().copy()
##goal = point(3,17)
testdivider = 1
self.goal = point(int(newHeight / testdivider * 0.8),
int(newWidth / testdivider * 0.8))
#cv2.waitKey(0)
##mapper.printMoveGrid()
print "STARTIN LOOP"
moveId = 0
Xlength = mapper.grid.shape[0] / testdivider
Ylength = mapper.grid.shape[1] / testdivider
#dstar = dstar3.DStar(Xlength,Ylength,goal)
dstar = dlite.Dlite(Xlength, Ylength, self.goal, robot)
print "Entering Loop"
testvar = 0
#for i in range(10):
while (robot.y != self.goal.y or robot.x != self.goal.x):
if testvar % 2 == 0:
newObs = obstacle.Obstacle(random.randint(0, height),
random.randint(0, width), 40)
self.map.placeObstacle(newObs, 3)
self.obsList.append(newObs)
#Place obstacles on map
self.map.updateObstacles(self.obsList)
#Morph the obstacles
self.map.updateMorph()
dliteimage = cv2.resize(self.map.getImage(),
(newWidth, newHeight))
cv2.imwrite('AGVCmap2.bmp', dliteimage)
imageMap.loadFile("AGVCmap2.bmp")
mapper.initalize(imageMap, robot)
moveGrid = imageMap.convertToGrid().copy()
testvar = testvar + 1
moveId = moveId + 1
print moveId
if path.pathIsBroken(mapper.grid):
path.restart()
print "The path is broken"
# dstar2.dstar(mapper, robot, goal, path)
dstar.dstar(mapper, robot, self.goal, path)
#dlite.dstar(robot,goal,path)
# # DstarLite.dstar(mapper, robot, goal, path)
# astar.astar(mapper, robot, goal, path)
pathNode = path.getNextMove()
robot.x = pathNode.x
robot.y = pathNode.y
mapper.moveGrid[pathNode.x][pathNode.y] = "1"
#mapper.printMoveGrid()
self.vehicle.x = pathNode.x
self.vehicle.y = pathNode.y
self.vehicle.addPosition(pathNode.x, pathNode.y)
mapper.updateMap(robot)
#raw_input("TEST")
cv2.imshow('AGVC Map', self.map.getMap())
cv2.imshow('AGVC Map Morphed', self.map.getImage())
for i in range(len(self.vehicle.positions)):
self.map.placeRobot(
self.vehicle.positions[i][1] * height / newHeight,
self.vehicle.positions[i][0] * width / newWidth,
self.vehicle.radius)
nPoints = len(self.vehicle.positions)
points = np.array(self.vehicle.positions) * height / newHeight
#points = np.random.rand(nPoints,2)*200
xpoints = [p[0] for p in points]
ypoints = [p[1] for p in points]
xvals, yvals = self.bezier_curve(points, nTimes=1000)
for i in range(len(xvals)):
self.map.placeRobot(int(yvals[i]), int(xvals[i]),
self.vehicle.radius * 2)
self.map.updateActObstacles(self.obsList)
cv2.waitKey(0)