Pepperくんの3Dセンサーを利用して写真の背景除去を行うPythonスクリプト

blob_detect.py

import math, time

class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)

    def onLoad(self):
        #put initialization code here
        self.memory = ALProxy('ALMemory')
        self.video = ALProxy('ALVideoDevice')
        self.frameManager = ALProxy("ALFrameManager")
        self.seg = ALProxy("ALSegmentation3D")
        self.seg.setDeltaDepthThreshold(0.01)
        self.log('threshold=%f' % self.seg.getDeltaDepthThreshold())
        self.tabletService = self.session().service("ALTabletService")
        self.lastTime = 0
        path = self.frameManager.getBehaviorPath(self.behaviorId)
        l = path.split('/')[:-1] + ['html']
        self.recordFolder = '/'.join(l)
        self.photo = ALProxy("ALPhotoCapture")
        # VGA 640*480
        self.photo.setResolution(2)
        self.photo.setColorSpace(0)
        self.photo.setPictureFormat("jpg")
        self.bm = ALProxy("ALBehaviorManager")

    def onUnload(self):
        #put clean-up code here
        pass

    def _getAppName(self):
        if self.frameManager:
            behaviorPath = os.path.normpath(self.frameManager.getBehaviorPath(self.behaviorId))
            appsFolderFragment = os.path.join("PackageManager", "apps")
            if not (appsFolderFragment in behaviorPath):
                self.logger.error("appsFolderFragment is not in behaviorPath")
            fragment = behaviorPath.split(appsFolderFragment, 1)[1]
            fragment = fragment.split('/')[1]
            self.logger.info(fragment)
            return fragment.lstrip("\\/")
        else:
            self.logger.warning("No ALFrameManager")

    def _getAbsoluteUrl(self, partial_url):
        subPath = os.path.join(self._getAppName(), os.path.normpath(partial_url).lstrip("\\/"))
        # We create TabletService here in order to avoid
        # problems with connections and disconnections of the tablet during the life of the application
        return "http://%s/apps/%s" %(self.tabletService.robotIp(), subPath.replace(os.path.sep, "/"))

    def onInput_onStart(self):
        now = time.time()
        #if now - self.lastTime < 10:
        #    return
        self.lastTime = now

        blobData = self.memory.getData('Segmentation3D/BlobsList')
        blobList = blobData[1]
        self.log('detected %d blobs' % len(blobList))
        self.log('=======')
        targetDist = 100000000
        targetPos = None
        targetSize = None
        targetTop = None
        for blob in blobList:
            roi = blob[1]
            dist = blob[2]
            xAngle = roi[0] + (math.pi*5.0/180.0)
            yAngle = roi[1] + (math.pi*5.0/180.0)
            self.log('xAngle %f => %f' % (roi[0], xAngle))
            self.log('yAngle %f => %f' % (roi[1], yAngle))
            pos = self.video.getImagePositionFromAngularPosition(2, [xAngle, yAngle])
            size = self.video.getImageSizeFromAngularSize(2, [roi[2], roi[3]])
            self.log('pos=%s size=%s dist=%f' % (str(pos), str(size), dist))
            if dist < targetDist:
                targetDist = dist
                targetPos = pos
                targetSize = size
                targetTop = blob[4]
        self.log('=======')
        if targetSize[0] < 0.2 or targetSize[1] < 0.2:
            self.log('image is too small')
            self.onStopped()
            return
        if targetDist > 1.0:
            self.log('too far')
            self.onStopped()
            return

        self.log('found target')
        self.log('pos=%s size=%s dist=%f' % (str(targetPos), str(targetSize), targetDist))

        self.photo.takePicture("/home/nao/", "image")
        self.bm.startBehavior('Stand/BodyTalk/Thinking/ThinkingLoop_1')
        time.sleep(1.0)
        im = Image.open("/home/nao/image.jpg")
        imageWidth = im.size[0]
        imageHeight = im.size[1]

        MARGIN = 20
        l = (int)(targetPos[0]*imageWidth) - MARGIN
        t = (int)(targetPos[1]*imageHeight) - MARGIN
        w = (int)(targetSize[0]*imageWidth) + 2* MARGIN
        h = (int)(targetSize[1]*imageHeight) + 2 * MARGIN
        self.log('crop (%d %d %d %d)' % (l, t, l+w, t+h))
        im = im.crop((l, t, l+w, t+h))
        im.save(os.path.join(self.recordFolder, "camImage.png"), "PNG")

        url = self._getAbsoluteUrl("camImage.png");
        self.log(url)
        self.tabletService.showImage(url+"?="+str(random.randint(0,100000)))
        self.onStopped()

    def onInput_onStop(self):
        self.onUnload() #it is recommended to reuse the clean-up as the box is stopped
        self.onStopped() #activate the output of the box