Exemple #1
0
    void run()
    {
        Bottle *pTarget=port.read(false);
        if (pTarget!=NULL)
        {
            if (pTarget->size()>2)
            {
                if (pTarget->get(2).asInt()!=0)
                {
                    Vector px(2);
                    px[0]=pTarget->get(0).asDouble();
                    px[1]=pTarget->get(1).asDouble();

                    // track the moving target within
                    // the camera image
                    igaze->lookAtMonoPixel(0,px);   // 0: left image plane is used, 1: for right
                }
            }
        }
    }
Exemple #2
0
    bool updateModule()
    {
        now = Time::now();
        double dT = now - prev;

        Bottle ispeakreply, ispeakcmd;
        ispeakcmd.add("stat");
        speechStatusPort.write(ispeakcmd, ispeakreply);
        if (prevStr=="quiet" && ispeakreply.toString()=="speaking")         // starting to speak
        {
            if(firstspeech)     // there was no previous writing
            {
                gazelength=0.0;
             //   prevgazetime = Time::now();
            }
            else  // sum up previous writing
            {
                if(prevgaze)
                {
                    gazelength = gazelength + Time::now() - gazestart;
                    gazestart = Time::now();
//                    gazelength = gazelength + Time::now() - prevgazetime;
//                    prevgazetime = Time::now();
                }
                stringstream logstream1;
                logstream1 << "Gaze time is: " << gazelength;
                log(now, logstream1.str());
                gazelength=0.0;
            }
            log(now,"Starting to speak.");
            firstspeech=0;
        }
        if (prevStr=="speaking" && ispeakreply.toString()=="quiet")         // starting to write
        {
            if(prevgaze)
            {
                gazelength = gazelength + Time::now() - gazestart;
                gazestart = Time::now();
                //                gazelength = gazelength + Time::now() - prevgazetime;
            }
            stringstream logstream1;
            logstream1 << "Gaze time is: " << gazelength;
            log(now, logstream1.str());
//            log(now,"Done speaking.");
            log(now,"Starting to write.");
            gazelength=0.0;
        }

        Bottle* out=gazeIn.read(false);
        Bottle mutgaze;
        if(out!=NULL)                               // is there gaze information
        {
            //cerr << out->toString() << endl;
            Bottle& faces = out->findGroup("faces");
            //cerr << faces.toString() << endl;
            Bottle& face = faces.findGroup("face");
            mutgaze = face.findGroup("mutualgaze");
            Bottle& facerect = face.findGroup("facerect");
            int facex = facerect.get(1).asInt();
            int facey = facerect.get(2).asInt();
            int faceheight = facerect.get(3).asInt();
            int facewidth = facerect.get(4).asInt();
            int facecenterx = facex + facewidth+10;
            int facecentery = facey + faceheight;

            if(faceheight>0)
            {
                facecounter++;
                if(facecounter==30)                // the robot updates its target of gaze every second (facecounter==100)
                {
                    facecounter=0;
                    Vector face_center;
                    face_center.resize(2);
                    face_center[0]=facecenterx;
                    face_center[1]=facecentery-60;
                    if(frame_counter % 20 == 0)
                    {
                        if(motorson)
                        {
				cout << "Face center: " << face_center[0] << " " << face_center[1] << endl;
                            igaze->lookAtMonoPixel(0,face_center);
                        }
                    }
                }
            }
        }

        if(state==1 || state==4 || state==7 || state==10)
        {
            if(dT>13.0 && initstate==1)
            {
	    	if(english)
		{
			saythis("Hello I'm eye cub.");
                	initstate=1;
                	prev = Time::now();
                	state++;
                	gazelength=0.0;
		}
///			saythis("Hello I'm eye cub. Today we will perform a dictation.");
		else
			saythis("Ciao, sono aicab. Oggi faremo un dettato.");
                if(state==1 || state==10)
                {
                    if(order==1)
                    {
			if(english)
                        	
                        	saythis("This is procedure Alpha.");
			
			else			
			        saythis("Questa è la procedura Alpha.");
                        withgaze=0;
                    }
                    else
                    {
			if(english)
                        	saythis("This is procedure Beta.");
			else                        
				saythis("Questa è la procedura Beta.");
                        withgaze=1;
                    }

                }
                if(state==4 || state == 7)
                {
                    if(order==1)
                    {
			if(english)
	                        saythis("This is procedure Beta.");
			else
				saythis("Questa è la procedura Beta.");
                        withgaze=1;
                    }
                    else
                    {
                        if(english)
				saythis("This is procedure Alpha.");
			else
			        saythis("Questa è la procedura Alpha.");
	
                        withgaze=0;
                    }
                }
                initstate++;
                gazelength=0.0;
            }
            if(dT>22.0 && initstate==2)
            {
/*		if(english)
                	saythis("Please; write on the board the following sentences.");
		else                
			saythis("Per favore scriva sulla lavagna le seguenti frasi."); */
		if(english)
			saythis("Please, take your pen and be ready to write down the following sentences.");
		else
			saythis("Per favore, prendi il pennarello e scrivi le frasi che ti detto.");
                initstate++;
                gazelength=0.0;
            }
            if(dT>32.0 && initstate==3)
            {
                initstate=1;
                prev = Time::now();
                state++;
                gazelength=0.0;
            }
            firstspeech=1;
        }

        if(state==3 || state==6 || state==9)        // waiting for experimenter's button press to continue with next stage
        {
            log(now, "Waiting.");
            std::cin.clear();
            std::fflush(stdin);
            std::cin.get ();    // get c-string
            log(now, "Done waiting.");
            prev = Time::now();
            state++;
            gazelength=0.0;
	    yarp::sig::Vector azelr(3);
///	    azelr[0] = -30.0;
///	    azelr[1] = 0.0;
	    azelr[0] = 0.0;
	    azelr[1] = 20.0;
	    azelr[2] = 0.0;
            igaze->lookAtAbsAngles(azelr);
        }

        if(state==2 || state==5 || state==8 || state==11)
        {
            if(withgaze)                            // procedure Beta - with gaze
            {
                if(prevyes || initstate==1)
                {
                    initstate++;

/*                    stringstream logstream2;
                    logstream2 << "Gaze time is: " << gazelength;
                    log(now, logstream2.str());
*/
                    std::string hullo;
                    hullo = list.back();
                    list.pop_back();
                    saythis(hullo);
///		    saythis("Hi. How are you?");
                    prev = Time::now();
                    prevyes=0;
                }
            }
            else                                // procedure Alpha - without gaze
            {
                if (prevStr=="speaking" && ispeakreply.toString()=="quiet")
                {
                    donespeaking=1;
//                    waittime = 2.6*(wordnumber);
//                    waittime = 2.1*(wordnumber);
		    waittime = 0.55 * charnumber;          // 0.55 = 2.4
                    stringstream logstream;
                    logstream << "Wait time is: " << waittime;
                    log(now, logstream.str());
                    prev = Time::now();
                    dT = now - prev;
                }
                std::string hullo;
                if((donespeaking && dT>waittime) || initstate==1)
                {
                    initstate++;

/*                    stringstream logstream1;
                    logstream1 << "Gaze time is: " << gazelength;
                    log(now, logstream1.str());
*/

                    if(initstate==10)
                     {
                         state++;

                         stringstream logstream1;
                         logstream1 << "Gaze time is: " << gazelength;
                         log(now, logstream1.str());

                         gazelength=0.0;
			 if(english)
                         	saythis("End of procedure Alpha.");
			 else
				saythis("Fine della procedura Alpha.");
                         initstate=1;
                         prev = Time::now();
                     }
                     else
                     {
                        hullo = list.back();
                        wordnumber = std::count(hullo.begin(), hullo.end(), ' ')+1;
 			charnumber = hullo.length() - std::count(hullo.begin(), hullo.end(), '.');
                        list.pop_back();
                        saythis(hullo);
                        donespeaking=0;
                     }
                }
            }

            if(out!=NULL)                   // if message received from gaze module
            {
                int ismutualgaze = mutgaze.get(1).asInt();
                if(ismutualgaze==1 && prevgaze==0)
                {
                    log(now, "glance on");
                    gazestart = Time::now();
                }
                if(ismutualgaze==0 && prevgaze==1)
                {

                    log(now, "glance off");
                    gazelength = gazelength + Time::now() - gazestart;
                }
                prevgaze=ismutualgaze;
                if (ismutualgaze)
                {
                    if(withgaze && prevyes==0 && dT>3.0)
                    {
                        gazeCount++;
//                        if(gazeCount==10 && prevyes==0)
			gazelen = Time::now() - gazestart;
                        if(gazelen>0.15 && prevyes==0)
                        {
//                            log(now, "Gaze event.");
                            stringstream logstream1;
                            logstream1 << "Trigger gaze length: " << gazelen;
                            log(now, logstream1.str());

                            gazeCount=0;
                            prevyes = 1;
                            if(initstate==9)
                            {
//                                state++;

                                stringstream logstream1;
                                logstream1 << "Gaze time is: " << gazelength;
                                log(now, logstream1.str());

                                gazelength=0.0;
                                initstate=1;
				cout << "Blah" << endl; 
		list.push_back("Benvenuti.");
		list.push_back("Io sono AICAB.");
		list.push_back("Sono felice di vederti.");
		list.push_back("Ciao.");
		list.push_back("Come va?");
		list.push_back("Questa e casa mia.");
		list.push_back("Io sono un robot.");
		list.push_back("Ciao.");
/*				if(english)
                                	saythis("End of procedure Beta");
				else
					saythis("Fine della procedura Beta.");
                                prev = Time::now();
                                if(order==1)
                                    withgaze=0;
                                else
                                    withgaze=1;*/
                            }
                        }
                    }
                }
            }

        }
        prevStr = ispeakreply.toString();
        return true;
    }
Exemple #3
0
    virtual void run()
    {
        // top-down input: handled at the highest priority
        if (Bottle *topDownInput=portTopDown.read(false))
        {
            Vector px(2);
            px[0]=topDownInput->get(0).asInt();
            px[1]=topDownInput->get(1).asInt();
            igaze->lookAtMonoPixel(eye=="left"?0:1,px,eyeDist);

            armRest();
            setFace(FACE_EXPR_TOPDOWN);

            state=STATE_TOPDOWN;
            centroidUpdate=0;
            t0=Time::now();

            fprintf(stdout,"Top-Down gazing at: (%d,%d) pixel\n",(int)px[0],(int)px[1]);
        }

        if (state==STATE_TOPDOWN)
            if (timer>=holdoff)
                resetState();

        // get inputs
        ImageOf<PixelBgr> *pImgIn=portImgIn.read(false);        

        // seek for faces
        if ((state==STATE_SEARCH) && (timer>timerThres))
        {            
            Vector ang(3);
            ang[0]=Rand::scalar(GAZE_AZI_MIN,GAZE_AZI_MAX);
            ang[1]=Rand::scalar(GAZE_ELE_MIN,GAZE_ELE_MAX);
            ang[2]=Rand::scalar(GAZE_VER_MIN,GAZE_VER_MAX);

            igaze->lookAtAbsAngles(ang);

            timerThres=Rand::scalar(GAZE_TIMER_MIN,GAZE_TIMER_MAX);
            t0=Time::now();

            fprintf(stdout,"Gazing at: (%.1f,%.1f,%.1f) deg\n",ang[0],ang[1],ang[2]);
        }

        // process camera image
        if (pImgIn)
        {            
            ImageOf<PixelBgr> &imgOut=portImgOut.prepare();
            imgOut=*pImgIn;
            IplImage *pVideoFrame=(IplImage*)imgOut.getIplImage();

            CvSeq *pFaceRectSeq=fd.detect(pVideoFrame);

            if (state==STATE_SEARCH)
            {
                if (pFaceRectSeq)
                    if (pFaceRectSeq->total)
                    {
                        CvRect *pFaceRect=(CvRect*)cvGetSeqElem(pFaceRectSeq,0);
                        drawFaceBox(pVideoFrame,pFaceRect);
                        faceCentroid=getCentroid(pFaceRect);

                        setFace(FACE_EXPR_FOUND);
                        enableFaceExprTrack=true;
                        t0=Time::now();

                        state=STATE_TRACK;
                    }
            }
            else if (state==STATE_TRACK)
            {
                if (pFaceRectSeq)
                    if (pFaceRectSeq->total)
                    {
                        CvRect *pFaceRect=getMostLikely(pFaceRectSeq,&faceCentroid);
                        drawFaceBox(pVideoFrame,pFaceRect);
                        faceCentroid=getCentroid(pFaceRect);

                        if (enableFaceExprTrack)
                        {
                            setFace(FACE_EXPR_TRACK);
                            enableFaceExprTrack=false;
                        }
                    }
                    else
                        resetState(true);
                else
                    resetState(true);
            }

            portImgOut.write();
        }

        // send centroid and command arm
        if (centroidUpdate)
        {
            Vector px(2);
            px[0]=faceCentroid.x;
            px[1]=faceCentroid.y;
            igaze->lookAtMonoPixel(eye=="left"?0:1,px,eyeDist);

            armTrack();

            centroidUpdate=0;            
        }

        faceExprHandling();

        timer=Time::now()-t0;
    }