Search code examples
pythonnao-robotchoregraphe

Subscribing and unsubscribing to ALVideoDevice in Python Script Box in Choregraphe not working


We are some students in IT and we are working on NAO (so we are very beginner). We want to implement a pathfinding by line detection on the ground with NAO cameras. We worked on a Python Script Box in Choregraphe for that purpose but we are not able to suscribe and unsubscribe correctly to the "ALVideoDevice"

Here is the code of the Python Script Box :

import sys
import numpy as np
import cv2
from naoqi import ALProxy
import PIL
from PIL import Image
from PIL import ImageDraw
from PIL import ImageFont
import vision_definitions as vd

def avg_lines_angles(lines):
    angle_sum = 0
    for line in lines:
        angle_sum = angle_sum + get_angle(line[0])
    return angle_sum/len(lines)

def get_angle(tupl):
    x1,y1,x2,y2 = tupl
    dy = (y2-y1)
    dx = (x2-x1)
    angle = np.arctan2(dy,dx)
    if angle < 0:
        angle = angle + np.pi
    return angle

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

    def onLoad(self):
        self.nameID = "test_subscribe"
        self.videoDevice = ALProxy('ALVideoDevice')
        self.captureDevice = self.videoDevice.subscribe(self.nameID, vd.k4VGA, vd.kRGBColorSpace, 20)

    def onUnload(self):
        #put clean-up code here
        self.videoDevice.unsubscribe(self.nameID)

    def onInput_onStart(self):
        index = None
        # get image
        result = self.videoDevice.getImageRemote(self.captureDevice);
        if result == None:
            print 'cannot capture.'
            #self.onUnload()
            self.onError()
        # show image
        else:
            basewidth = 600
            img = Image.fromstring("RGB",(result[0],result[1]),result[6])
            self.videoDevice.releaseImage(self.nameID)
            img = img.crop((img.size[0]/4,3*img.size[1]/5,3*img.size[0]/4,img.size[1]))
            wpercent = (basewidth/float(img.size[0]))
            hsize = int((float(img.size[1])*float(wpercent)))
            img = img.resize((basewidth,hsize), Image.ANTIALIAS)
            gray = img.convert("L")
            gray = np.array(gray)
            edges = cv2.Canny(gray,50,150,apertureSize = 3)
            cv2.imwrite("/home/nao/Pictures/debug.jpg",edges)
            lines = cv2.HoughLinesP(edges,1,np.pi/180,60,minLineLength=50,maxLineGap=30)
            try:
                angle = avg_lines_angles(lines)
                self.onStopped(angle)
            except:
                self.onError()
            #self.onUnload()


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

We are subscribing to the service on loading of the class and we are unsubscribing when the class is unload. The thing that is really weird is that error on unsubscribing :

[ERROR] vision.videodevice :unsubscribe:0 Can't unsubscribe "test_subscribe", subscriber is unknown.

Our box is in a loop the number of subscribers limits what we can do. We are able to get the images from the ALVideoDevice, so we are subscribed. But using the same name for unsubscribing is not working at all. We did not find anything in the Python SDK API, just some tutorials that describe exactly what we are doing ( Subscribe -> Code that use ALVideoDevice -> Unsubscribe )


Solution

  • Your problem is that you're unsubscribing using the name of the subscriber, whereas you're supposed to unsubscribe using a handle, that is given to you when you subscribe - so in your case, you should be doing

    self.videoDevice.unsubscribe(self.captureDevice)