RRTstar_Scan1.py 文件源码

def markVisibleArea(self, originalImg):
        visibleImg = np.zeros(self.img.shape, np.uint8)
        x, y = self.current[0], self.current[1]
        lx, ly = -200, -200 #last coordinates
        points = []
        for i in range(1083):
            nx, ny = self.findNearestObstacle(originalImg, x, y, i/3)
            #print nx, ny
            nx = int(nx)
            ny = int(ny)
            points.append((ny, nx))
            if i != 0:
                cv2.line(visibleImg, (ny, nx), (ly, lx), 100, 1)
            lx, ly = nx, ny
        h, w = visibleImg.shape

        mask = np.zeros((h+2, w+2), np.uint8)
        cv2.floodFill(visibleImg, mask, (y, x), 100)
        for i in points:
            cv2.circle(visibleImg, i, 3, 255, 6)

        self.img = cv2.bitwise_or(self.img, visibleImg)