bool threadInit()
{
string name=rf.find("name").asString().c_str();
double period=rf.find("period").asDouble();
setRate(int(1000.0*period));
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator (icubSim) is running
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options:
// --robot icubSim --context cameraCalibration/conf --config icubSimEyes.ini
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!clientGaze.open(optGaze))
return false;
// open the view
clientGaze.view(igaze);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_id);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
port.open(("/"+name+"/target:i").c_str());
return true;
}
virtual bool threadInit()
{
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator is running;
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options: "--from configSim.ini"
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/gaze_client");
if (!clientGaze.open(optGaze))
return false;
// open the view
clientGaze.view(igaze);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_id);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
// put the gaze in tracking mode, so that
// when the torso moves, the gaze controller
// will compensate for it
igaze->setTrackingMode(true);
// print out some info about the controller
Bottle info;
igaze->getInfo(info);
fprintf(stdout,"info = %s\n",info.toString().c_str());
Property optTorso("(device remote_controlboard)");
optTorso.put("remote","/icubSim/torso");
optTorso.put("local","/torso_client");
if (!clientTorso.open(optTorso))
return false;
// open the view
clientTorso.view(ienc);
clientTorso.view(ipos);
ipos->setRefSpeed(0,10.0);
fp.resize(3);
state=STATE_TRACK;
t=t0=t1=t2=t3=t4=Time::now();
return true;
}
virtual bool threadInit()
{
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator (icubSim) is running
// 2 - the gaze server iKinGazeCtrl is running and
// launched with --robot icubSim option
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/gaze_client");
clientGaze=new PolyDriver;
if (!clientGaze->open(optGaze))
{
delete clientGaze;
return false;
}
// open the view
clientGaze->view(igaze);
// set trajectory time:
// we'll go like hell since we're using the simulator :)
igaze->setNeckTrajTime(0.4);
igaze->setEyesTrajTime(0.1);
// put the gaze in tracking mode, so that
// when the torso moves, the gaze controller
// will compensate for it
/*pramod modified starts
igaze->setTrackingMode(true);
Property optTorso("(device remote_controlboard)");
optTorso.put("remote","/icubSim/torso");
optTorso.put("local","/torso_client");
clientTorso=new PolyDriver;
if (!clientTorso->open(optTorso))
{
delete clientTorso;
return false;
}
// open the view
clientTorso->view(ienc);
clientTorso->view(ipos);
ipos->setRefSpeed(0,10.0);
pramod modified ends */
fp.resize(3);
state=STATE_TRACK;
t=t0=t1=t2=t3=t4=Time::now();
return true;
}
virtual bool threadInit() {
/* Read in parameters from resource finder */
loadParams();
/* Start up gaze control client interface */
Property option("(device gazecontrollerclient)");
option.put("remote","/iKinGazeCtrl");
option.put("local","/client/gaze");
clientGazeCtrl.open(option);
gaze=NULL;
if (clientGazeCtrl.isValid()) {
clientGazeCtrl.view(gaze);
} else {
printf("could not initialize gaze control interface, failing...\n");
return false;
}
//set gaze control interface params
gaze->setNeckTrajTime(neckTT);
gaze->setEyesTrajTime(eyeTT);
gaze->bindNeckPitch(-30,30);
gaze->bindNeckYaw(-25,25);
gaze->bindNeckRoll(-10,10);
/* Start up arm cartesian control client interface */
Property optiona("(device cartesiancontrollerclient)");
if (armInUse) {
string tmpcrname = "/" + robotname + "/cartesianController/right_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/right_arm");
}
else {
string tmpcrname = "/" + robotname + "/cartesianController/left_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/left_arm");
}
clientArmCart.open(optiona);
carm = NULL;
if (clientArmCart.isValid())
{
clientArmCart.view(carm);
} else {
printf("could not initialize arm cartesian control interface, failing...\n");
return false;
}
// set trajectory time
carm->setTrajTime(trajTime); //slow for safety
// get the torso dofs
Vector newDof, curDof;
carm->getDOF(curDof);
newDof=curDof;
//enable torso pitch and yaw, also wrist movements
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
newDof[7]=1;
newDof[8]=1;
newDof[9]=1;
// impose some restriction on the torso pitch
double tpmin, tpmax;
carm->getLimits(0,&tpmin,&tpmax);
carm->setLimits(0,tpmin,maxPitch);
// send the request for dofs reconfiguration
carm->setDOF(newDof,curDof);
string lname, rname;
Property doption;
doption.put("device", "remote_controlboard");
lname = "/"+name+"/torso"; rname = "/"+robotname+"/torso";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotTorso.open(doption);
if (!robotTorso.isValid()) {
printf("could not initialize torso control interface, failing...\n");
return false;
}
robotTorso.view(tPos); robotTorso.view(tEnc);
lname = "/"+name+"/head"; rname = "/"+robotname+"/head";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotHead.open(doption);
if (!robotHead.isValid()) {
printf("could not initialize head control interface, failing...\n");
return false;
}
robotHead.view(hPos); robotHead.view(hEnc);
lname = "/"+name+"/"+armname+"_arm"; rname = "/"+robotname+"/"+armname+"_arm";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotArm.open(doption);
if (!robotArm.isValid()) {
printf("could not initialize arm control interface, failing...\n");
return false;
}
robotArm.view(aPos); robotArm.view(aEnc);
for (int i=0; i<7; i++) {
aPos->setRefSpeed(i, 10.0);
}
/* Create and open ports */
cportL=new ClickPort(bfL);
string cplName="/"+name+"/clk:l";
cportL->open(cplName.c_str());
cportL->useCallback();
cportR=new ClickPort(bfR);
string cprName="/"+name+"/clk:r";
cportR->open(cprName.c_str());
cportR->useCallback();
susPort=new Port;
string suspName="/"+name+"/sus:o";
susPort->open(suspName.c_str());
return true;
}
/*
* Configure function. Receive a previously initialized
* resource finder object. Use it to configure your module.
* Open port and attach it to message handler.
*/
bool configure(yarp::os::ResourceFinder &rf)
{
/*
list.push_back("The market is open."); // 4
list.push_back("The orange is sweet."); // 4
list.push_back("I love to play guitar."); // 5
list.push_back("He is in the kitchen."); // 5
list.push_back("I can speak English."); // 4
list.push_back("He is going home."); // 4
list.push_back("Tom is a funny man."); // 5
list.push_back("I have three apples."); // 4
list.push_back("The music; was good."); // 4
list.push_back("I am not a robot."); // 5
list.push_back("She is very pretty."); // 4
list.push_back("This song is great."); // 4
list.push_back("My friend has a horse."); // 5
list.push_back("The apple tasted good."); // 4
list.push_back("I like red flowers."); // 4
list.push_back("My horse runs very fast."); // 5
list.push_back("I have a nice house."); // 5
list.push_back("Jill wants a doll."); // 4
list.push_back("I have a computer."); // 4
list.push_back("Tom is stronger than Dan."); // 5
list.push_back("We, sing; a song."); // 4
list.push_back("I was very happy yesterday."); // 5
list.push_back("He eats white bread."); // 4
list.push_back("Jack wants a toy."); // 4
list.push_back("She is in the shower."); // 5
list.push_back("The baby plays with toys."); // 5
list.push_back("She is a teacher."); // 4
list.push_back("I have a nice box."); // 5
list.push_back("You look very happy."); // 4
list.push_back("The baby fell asleep."); // 4
list.push_back("They are; my friends."); // 4
list.push_back("I went to school."); // 4
*/
english = 1;
if(english)
{
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.");
/* list.push_back("Hi, how are you?");
list.push_back("Hello.");
list.push_back("I'm looking at you.");
list.push_back("How is it going?");
list.push_back("Hello, I'm eye cub.");
list.push_back("Welcome to my home.");
list.push_back("Hi there.");
list.push_back("Hello.");
*/
/* list.push_back("We sing a song.");
list.push_back("I was very happy yesterday.");
list.push_back("I have a computer.");
list.push_back("Jill wants a doll.");
list.push_back("He eats white bread.");
list.push_back("I love to play guitar.");
list.push_back("He is in the kitchen.");
list.push_back("She has a nice bike.");
list.push_back("I went to school.");
list.push_back("Tom is stronger than Dan.");
list.push_back("She is a teacher.");
list.push_back("I like red flowers.");
list.push_back("The music was good.");
list.push_back("The apple tasted good.");
list.push_back("You look very happy.");
list.push_back("I have three apples.");
list.push_back("Jack wants a toy.");
list.push_back("The baby plays with toys.");
list.push_back("I have a nice box.");
list.push_back("This song is great.");
list.push_back("Tom is a funny man.");
list.push_back("My friend has a horse.");
list.push_back("The baby fell asleep.");
list.push_back("I can speak English.");
list.push_back("I am not a robot.");
list.push_back("My horse runs very fast.");
list.push_back("They are going home.");
list.push_back("She is very pretty.");
list.push_back("The market is open.");
list.push_back("She is in the shower.");
list.push_back("They are; my friends.");
list.push_back("The apple is sweet.");*/
}
else
{
list.push_back("Il bar è aperto.");
list.push_back("Mi piace molto ballare.");
list.push_back("Non bevo il caffè.");
list.push_back("Marco ha tanti cani.");
list.push_back("Il mare è calmo.");
list.push_back("Il cane abbaia spesso.");
list.push_back("Sono allergico al latte.");
list.push_back("La sedia è in camera.");
list.push_back("La luce è rossa.");
list.push_back("Lui ha sempre ragione.");
list.push_back("Giovedí scorso era festa.");
list.push_back("Oggi splende il sole.");
list.push_back("Ho comprato il pane.");
list.push_back("Guido tutti i giorni.");
list.push_back("Vado spesso al mare.");
list.push_back("La notte è buia.");
list.push_back("Mi piace il gelato.");
list.push_back("Devo scrivere un tema.");
list.push_back("Lei ama cucire.");
list.push_back("Io ballo la polka.");
list.push_back("Gino canta molto bene.");
list.push_back("Vado a dormire presto.");
list.push_back("La lettera è firmata.");
list.push_back("Gioco spesso a carte.");
list.push_back("Mi piace l'opera.");
list.push_back("Ho mangiato le mele.");
list.push_back("Sua nonna sta bene.");
list.push_back("Ho una maglia blu.");
list.push_back("Il piatto è sul tavolo.");
list.push_back("La giornata è piovosa.");
list.push_back("Il film dura due ore.");
list.push_back("La bottiglia è piena.");
}
gazeCount=0;
speech_counter=0;
online=1;
prev = Time::now();
waittime=5.0;
withgaze=1;
prevyes=0;
state=1;
order=2;
initstate=1;
prevStr = "quiet";
donespeaking=1;
wordnumber=0;
charnumber=0;
prevgaze=0;
facecounter=0;
gazelength=0.0;
gazelen=0.0;
motorson=1;
firstspeech=0;
if(motorson==1)
{
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/icub_eyetrack/gaze");
printf("\nHello.\n");
if (!clientGaze.open(optGaze))
{
printf("\nGAZE FAILED\n");
return false;
}
else
printf("\nGAZE OPEN\n");
clientGaze.view(igaze);
igaze->blockNeckRoll(0.0);
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
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);
}
handlerPort.open("/dictationcontroller");
gazeIn.open( "/dictationcontroller/gaze:i" );
speechOut.open( "/dictationcontroller/speech:o" );
logOut.open( "/dictationcontroller/log:o" );
attach(handlerPort);
Network::connect("/dlibgazer/out", "/dictationcontroller/gaze:i");
Network::connect("/dictationcontroller/speech:o", "/iSpeak");
speechStatusPort.open("/dictationcontroller/iSpeakrpc");
Network::connect("/dictationcontroller/iSpeakrpc", "/iSpeak/rpc");
frame_counter=0;
cout<<"Done configuring!" << endl;
return true;
}
bool configure(ResourceFinder &rf)
{
std::string name = "MakeItRoll";
// open a client interface to connect to the gaze server
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
return false;
// open the view
drvGaze.view(igaze);
if(!igaze)
return false;
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_gaze);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
// open a client interface to connect to the arm cartesian control server
Property optArm("(device cartesiancontrollerclient)");
optArm.put("remote","/icubSim/cartesianController/right_arm");
optArm.put("local",("/"+name+"/cartesian/right_arm").c_str());
if (!drvArm.open(optArm))
return false;
// open the view
drvArm.view(iarm);
// latch the controller context in order to preserve
// it after closing the module
iarm->storeContext(&startup_context_arm);
// set trajectory time
iarm->setTrajTime(1.0);
// get the torso dofs
Vector newDof, curDof;
iarm->getDOF(curDof);
newDof=curDof;
// enable the torso yaw and pitch
// disable the torso roll
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
// impose some restriction on the torso pitch
int axis=0; // pitch joint
double min, max;
// we keep the lower limit
iarm->getLimits(axis, &min, &max);
iarm->setLimits(axis, min, MAX_TORSO_PITCH);
iarm->setDOF(newDof,curDof);
// Opening the required streaming and rpc ports
imgLPortIn.open("/MakeItRoll/imgL:i");
imgRPortIn.open("/MakeItRoll/imgR:i");
imgLPortOut.open("/MakeItRoll/imgL:o");
imgRPortOut.open("/MakeItRoll/imgR:o");
rpcPort.open("/MakeItRoll/service");
attach(rpcPort);
return true;
}
