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