def planLawnmowerPath(self):
# #implement cellular decomposition, AStar and DMPP decomposition to fill contour
lastPoint = [self.easting, self.northing]
polyDir = self.LI.rotDirection(self.boundaryPoints)
crossingAr, crossingCount, tmppos = DMPP.sweep(self.boundaryPoints, self.sweepAngle, self.den)
cells, neighbours = DMPP.createCells(crossingAr,crossingCount,self.den,self.sweepAngle)
shrunkCells = DMPP.shrinkCells(cells,self.den)
posinpoly = False
for sc in shrunkCells:
if self.LI.isPointInPolygon(lastPoint[0],lastPoint[1],sc) ==True:
posinpoly = True
break
# else find nearest point that is
if posinpoly == False:
eas,nor = DMPP.crossingPointClosestCell(lastPoint[0],lastPoint[1],shrunkCells)
lastPoint = [eas,nor]
path = np.array([lastPoint])
for i in range(cells.shape[0]):
lastPoint = path[-1]
entry, transit = self.TSP.greedyCellTSP2(shrunkCells, lastPoint, self.boundaryPoints, self.den)
transit = np.asarray(transit)
if transit != []:
path = np.vstack([path,transit])
cellDir = self.LI.rotDirection(cells[entry[0]])
lp = DMPP.localPoly(cells[entry[0]],self.boundaryPoints, cellDir, polyDir)
subPath = DMPP.genLawnmower(cells[entry[0]],lp,entry[1],self.sweepAngle,self.den)
cells = np.delete(cells,entry[0],axis=0) #remove that cell from the list
shrunkCells = np.delete(shrunkCells, entry[0],axis=0)
path = np.vstack([path,subPath])
self.lawnmowerPath = path
bp = open((self.userHome + '/tmp/dmppPath.txt'),'w')
for point in self.lawnmowerPath:
row = str(round(point[0],2)) + ',' + str(round(point[1],2)) + '\n'
bp.write(row)
bp.close()
self.wpmsg.easting = path[:,0]
self.wpmsg.northing = path[:,1]
self.pubPath.publish(self.wpmsg)
transits = np.empty(4,dtype='object')
lps = np.empty(4,dtype='object')
for i in range(openCells.shape[0]):
lastPoint = path[-1]
entry, transit = TSP.greedyCellTSP2(openShrunkCells, lastPoint, boundingPolygon, dxy, 5)
# note due to cells being monotone to sweep direction, not to the normal of the transit direction, the transit path
# plan can cross the polygon boundary
# need to write function to hug boundary instead of crossing it
# Done - A* handles this
transit = np.asarray(transit)
if transit != []:
path = np.vstack([path,transit])
#tmpStr = openCells[entry[0]][entry[1]]
cellDir = LI.rotDirection(openCells[entry[0]])
lp = DMPP.localPoly(openCells[entry[0]],boundingPolygon)#, cellDir, polyDir)
subPath = DMPP.genLawnmower(openCells[entry[0]], boundingPolygon,lp,entry[1],sweepAngle,dxy)
openCells = np.delete(openCells,entry[0],axis=0) #remove that cell from the list
openShrunkCells = np.delete(openShrunkCells, entry[0],axis=0)
path = np.vstack([path,subPath])
#for plotting
print i
subPaths[i] = subPath
transits[i] = transit
lps[i] = lp
#Hull and 2D path