def genPolyWaypoints(self, pE, pN, dEN, wpEps):
     m = len(pN)
     path = [pE[0], pN[0]]
     for i in range(m):
         j = (i+1)% m
         pathseg = DMPP.wayPointsOnLine(pE[i], pN[i],pE[j], pN[j],dEN )
         path = np.vstack((path,pathseg))
     print path.shape
     done = False
     i = 0
     while done == False:
         j = (i+1)% len(path)
         ED = np.sqrt((path[i,0]-path[j,0])**2 + (path[i,1]-path[j,1])**2)
         if ED < wpEps:
             path = np.delete(path,i,0)
         else:
             i += 1
         if i == len(path):
             done = True
     return path
    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)
    travelled = np.load('travelled.npy')
    scannedPoints = np.load('scannedPoints.npy')
    hull = LI.externalHull(travelled,5.0)
    boundingPolygon = hull
    pos = travelled[-1]
    # fig = plt.figure()
    # plt.plot(travelled[:, 0], travelled[:, 1], color = 'blue', linestyle = '-', label = 'Travelled')
    # plt.plot(boundingPolygon[:, 0], boundingPolygon[:, 1], color = '#A901DB', linestyle = '-', label = 'Hull')
    # plt.show()

boundingPolygon = hull
sweepAngle = 0
dxy = 10
polyDir =  LI.rotDirection(boundingPolygon)

crossingAr, crossingCount, tmppos = DMPP.sweep(boundingPolygon, sweepAngle, dxy)
openCells, neighbours = DMPP.createCells(crossingAr,crossingCount,dxy,sweepAngle)
openShrunkCells = DMPP.shrinkCells(openCells,dxy, sweepAngle)
TSP = TSP.Tsp()
path = np.array([pos])

subPaths = np.empty(4,dtype='object')
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