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)
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 fig = plt.figure()