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
}
}
}
}
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;
}
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;
}