int main(int argc, char *argv[]) {
if (argc!=3) {
fprintf(stderr,"Call as %s X Y\n", argv[0]);
return 1;
}
Network yarp;
Node node("/yarp_add_int_client");
RpcClient client;
if (!client.open("add_two_ints")) {
fprintf(stderr,"Failed to open client\n");
return 1;
}
Bottle msg, reply;
msg.addInt(atoi(argv[1]));
msg.addInt(atoi(argv[2]));
if (!client.write(msg,reply)) {
fprintf(stderr,"Failed to call service\n");
return 1;
}
printf("Got %d\n", reply.get(0).asInt());
return 0;
}
int main(int argc, char *argv[]) {
if (argc<3) {
fprintf(stderr, "Please supply (1) a port name for the client\n");
fprintf(stderr, " (2) a port name for the server\n");
return 1;
}
Network yarp;
const char *client_name = argv[1];
const char *server_name = argv[2];
RpcClient port;
port.open(client_name);
int ct = 0;
while (true) {
if (port.getOutputCount()==0) {
printf("Trying to connect to %s\n", server_name);
yarp.connect(client_name,server_name);
} else {
Bottle cmd;
cmd.addString("COUNT");
cmd.addInt32(ct);
ct++;
printf("Sending message... %s\n", cmd.toString().c_str());
Bottle response;
port.write(cmd,response);
printf("Got response: %s\n", response.toString().c_str());
}
Time::delay(1);
}
}
int main(int argc, char *argv[]) {
if (argc!=3) {
fprintf(stderr,"Call as %s X Y\n", argv[0]);
return 1;
}
Network yarp;
RpcClient client;
yarp_test::AddTwoInts example;
client.promiseType(example.getType());
if (!client.open("/add_two_ints@/yarp_add_int_client")) {
fprintf(stderr,"Failed to open port\n");
return 1;
}
yarp_test::AddTwoInts msg;
yarp_test::AddTwoIntsReply reply;
msg.a = atoi(argv[1]);
msg.b = atoi(argv[2]);
if (!client.write(msg,reply)) {
fprintf(stderr,"Failed to call service\n");
return 1;
}
printf("Got %d\n", (int)reply.sum);
return 0;
}
void send()
{
if (emotions.getOutputCount()>0)
{
Bottle cmd, reply;
cmd.addVocab(Vocab::encode("set"));
cmd.addVocab(Vocab::encode("mou"));
cmd.addVocab(Vocab::encode(state.c_str()));
emotions.write(cmd,reply);
}
}
// Sends stop metering to RPC Service
__int64 StopMeteringData(RPC_CLIENT_HANDLE RpcClientHandle) {
RpcClient* client;
if (RpcClientHandle == NULL) {
return ERROR_INVALID_PARAMETER;
}
client = RetriveRpcClientFromHandle(RpcClientHandle);
if (client == NULL) {
return ERROR_INVALID_HANDLE;
}
return client->StopMetering();
}
// Updates sample rate of metering RPC Service
__int64 SetSampleRate(RPC_CLIENT_HANDLE RpcClientHandle, int rate) {
RpcClient* client;
if (RpcClientHandle == NULL) {
return ERROR_INVALID_PARAMETER;
}
client = RetriveRpcClientFromHandle(RpcClientHandle);
if (client == NULL) {
return ERROR_INVALID_HANDLE;
}
return client->SetSampleRate(rate);
}
// Starts metering service and blocks till metering is stopped
__int64 StartMeteringAndWaitForStop(RPC_CLIENT_HANDLE RpcClientHandle, __int64 samplePeriod) {
RpcClient* client;
if (RpcClientHandle == NULL) {
return ERROR_INVALID_PARAMETER;
}
client = RetriveRpcClientFromHandle(RpcClientHandle);
if (client == NULL) {
return ERROR_INVALID_HANDLE;
}
return client->StartMeteringAndWaitForStop(samplePeriod);
}
bool respond(const Bottle &command, Bottle &reply)
{
string cmd=command.get(0).asString().c_str();
int ack=Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (cmd=="clear")
{
LockGuard lg(mutex);
contour.clear();
floodPoints.clear();
go=flood3d=flood=false;
reply.addVocab(ack);
}
else if ((cmd=="go") || (cmd=="flood3d"))
{
if (portSFM.getOutputCount()==0)
reply.addVocab(nack);
else
{
LockGuard lg(mutex);
if (cmd=="go")
{
if (contour.size()>2)
{
flood=false;
go=true;
reply.addVocab(ack);
}
else
reply.addVocab(nack);
}
else if (cmd=="flood3d")
{
if (command.size()>=2)
spatial_distance=command.get(1).asDouble();
contour.clear();
floodPoints.clear();
flood=false;
flood3d=true;
reply.addVocab(ack);
}
}
}
else if (cmd=="flood")
{
if (command.size()>=2)
color_distance=command.get(1).asInt();
contour.clear();
floodPoints.clear();
flood=true;
reply.addVocab(ack);
}
else
RFModule::respond(command,reply);
return true;
}
void updateSim(const Vector &c_)
{
if ((c_.length()!=3) && (c_.length()!=4))
return;
Vector c=c_;
if (c.length()==3)
c.push_back(1.0);
c[3]=1.0;
c=Tsim*c;
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("set");
cmd.addString("ssph");
// obj #
cmd.addInt(1);
// position
cmd.addDouble(c[0]);
cmd.addDouble(c[1]);
cmd.addDouble(c[2]);
simPort.write(cmd,reply);
}
bool configure(ResourceFinder &rf)
{
rf1=rf;
if(algorithm==1)
loc5=new UnscentedParticleFilter();
else
loc5=new ScalingSeries();
loc5->configure(rf1);
state=0;
meas_given=1;
if(loc5->measurementsString!=1)
{
namePorts = rf.check("namePorts", Value("visual-localization"), "Getting module name").asString();
portIn.open("/"+namePorts+"/pnt:i");
rpcOut.open("/"+namePorts+"/toolFeat:rpc");
meas_given=0;
}
cout<<"meas_given "<<meas_given<<endl;
error_thres=loc5->error_thres;
cout<<"thresconf "<<error_thres<<endl;
rpcPort.open(("/"+namePorts+"/rpc").c_str());
attach(rpcPort);
delete loc5;
return true;
}
bool interruptModule()
{
cout<<"Interrupt caught!"<<endl;
cout<<endl;
handlerPort.interrupt();
objectsPort.interrupt();
return true;
}
void configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
string robot=rf.find("robot").asString().c_str();
emotions.open(("/"+name+"/emotions:o").c_str());
state="sur";
setRate(rf.check("period",Value(200)).asInt());
}
bool close()
{
if(meas_given!=1)
{
portIn.close();
rpcOut.close();
}
rpcPort.close();
return true;
}
bool interruptModule()
{
portDispIn.interrupt();
portDispOut.interrupt();
portOutPoints.interrupt();
portImgIn.interrupt();
portContour.interrupt();
portSFM.interrupt();
portRpc.interrupt();
return true;
}
bool close()
{
portDispIn.close();
portDispOut.close();
portOutPoints.close();
portImgIn.close();
portContour.close();
portSFM.close();
portRpc.close();
return true;
}
bool configure(ResourceFinder &rf)
{
Time::turboBoost();
string name=rf.find("name").asString().c_str();
port.open(("/"+name+":o").c_str());
if (!Network::connect(port.getName().c_str(),"/icubSim/world"))
{
cout<<"unable to connect to the world!"<<endl;
port.close();
return false;
}
x0=0.0;
y0=1.0;
z0=0.75;
init=true;
return true;
}
bool interruptModule()
{
if(meas_given!=1)
{
portIn.interrupt();
rpcOut.interrupt();
}
rpcPort.close();
if(state==1)
delete loc5;
return true;
}
void RpcService::send(const std::string &name, rpcproto::rpc *msg)
{
RpcClient *rc = 0;
std::map<std::string, RpcClient *>::iterator it = rpc_client_map_.find(name);
if (it == rpc_client_map_.end())
{
std::string host = svc::env()->get_server_value(name, "host");
std::string port = svc::env()->get_server_value(name, "port");
if (host == "" || port == "")
{
return;
}
std::string addr = "tcp://" + host + ":" + port;
rc = new RpcClient(name, addr);
rpc_client_map_[name] = rc;
}
else
{
rc = (*it).second;
}
std::string s;
msg->SerializeToString(&s);
rc->putq(s);
}
bool close()
{
iarm->stopControl();
iarm->restoreContext(startup_context);
drvCart.close();
ivel->stop(joints.size(),joints.getFirst());
for (size_t i=0; i<modes.size(); i++)
modes[i]=VOCAB_CM_POSITION;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
drvHand.close();
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
simPort.write(cmd,reply);
simPort.close();
}
if (gaze)
{
igaze->stopControl();
drvGaze.close();
}
igeo->stopFeedback();
igeo->setTransformation(eye(4,4));
drvGeomagic.close();
forceFbPort.close();
return true;
}
bool configure(ResourceFinder &rf)
{
portDispIn.open("/3d-points/disp:i");
portDispOut.open("/3d-points/disp:o");
portImgIn.open("/3d-points/img:i");
portOutPoints.open("/3d-points/pnt:o");
portContour.open("/3d-points/contour:i");
portSFM.open("/3d-points/SFM:rpc");
portRpc.open("/3d-points/rpc");
portContour.setReader(*this);
attach(portRpc);
homeContextPath=rf.getHomeContextPath().c_str();
downsampling=std::max(1,rf.check("downsampling",Value(1)).asInt());
spatial_distance=rf.check("spatial_distance",Value(0.004)).asDouble();
color_distance=rf.check("color_distance",Value(6)).asInt();
go=flood3d=flood=false;
return true;
}
void sendSolution(Matrix &R)
{
Bottle replyTF;
replyTF.clear();
replyTF.addString("setPose");
Bottle &R_bottle=replyTF.addList();
R_bottle.addDouble(R(0,0));
R_bottle.addDouble(R(0,1));
R_bottle.addDouble(R(0,2));
R_bottle.addDouble(R(0,3));
R_bottle.addDouble(R(1,0));
R_bottle.addDouble(R(1,1));
R_bottle.addDouble(R(1,2));
R_bottle.addDouble(R(1,3));
R_bottle.addDouble(R(2,0));
R_bottle.addDouble(R(2,1));
R_bottle.addDouble(R(2,2));
R_bottle.addDouble(R(2,3));
rpcOut.write(replyTF);
}
bool updateModule()
{
Bottle cmd,reply;
if (init)
{
cmd.addString("world");
cmd.addString("mk");
cmd.addString("ssph");
cmd.addDouble(0.03);
cmd.addDouble(x0);
cmd.addDouble(y0);
cmd.addDouble(z0);
cmd.addDouble(1.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cnt=0;
init=false;
}
else
{
double t=(cnt++)*getPeriod();
double phi=2.0*M_PI*(1.0/20.0)*t;
double dx=0.3*cos(phi);
double dy=0.3*sin(phi);
double dz=0.3*sin(phi);
cmd.addString("world");
cmd.addString("set");
cmd.addString("ssph");
cmd.addInt(1);
cmd.addDouble(x0+dx);
cmd.addDouble(y0+dy);
cmd.addDouble(z0+dz);
}
port.write(cmd,reply);
return true;
}
bool close()
{
handlerPort.close();
objectsPort.close();
igaze->stopControl();
igaze->restoreContext(startupGazeContextID);
igaze->deleteContext(startupGazeContextID);
igaze->deleteContext(currentGazeContextID);
if (clientGazeCtrl.isValid())
clientGazeCtrl.close();
icartLeft->stopControl();
icartLeft->restoreContext(startupArmLeftContextID);
icartLeft->deleteContext(startupArmLeftContextID);
icartLeft->deleteContext(currentArmLeftContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
icartRight->stopControl();
icartRight->restoreContext(startupArmRightContextID);
icartRight->deleteContext(startupArmRightContextID);
icartRight->deleteContext(currentArmRightContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
itorsoVelocity->stop();
if (clientTorso.isValid())
clientTorso.close();
return true;
}
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;
}
/*
* 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)
{
name = "visuoTactileRF";
robot = "icub";
modality = "1D";
verbosity = 0; // verbosity
rate = 20; // rate of the vtRFThread
//******************************************************
//********************** CONFIGS ***********************
//******************* NAME ******************
if (rf.check("name"))
{
name = rf.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (rf.check("robot"))
{
robot = rf.find("robot").asString();
yInfo("Robot is %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//***************** MODALITY *****************
if (rf.check("modality"))
{
modality = rf.find("modality").asString();
yInfo("modality is %s", modality.c_str());
}
else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());
//******************* VERBOSE ******************
if (rf.check("verbosity"))
{
verbosity = rf.find("verbosity").asInt();
yInfo("vtRFThread verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);
//****************** rate ******************
if (rf.check("rate"))
{
rate = rf.find("rate").asInt();
yInfo("vtRFThread working at %i ms", rate);
}
else yInfo("Could not find rate in the config file; using %i ms as default",rate);
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
vector<string> filenames;
int partNum=4;
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
{
if (rf.check("leftHand"))
{
string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("leftForeArm"))
{
string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightHand"))
{
string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightForeArm"))
{
string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
filenames.push_back(filePath);
}
}
}
else
{
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
filenames.push_back(filePath);
}
}
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
//*************** eyes' Resource finder ****************
ResourceFinder gazeRF;
gazeRF.setVerbose(verbosity!=0);
gazeRF.setDefaultContext("iKinGazeCtrl");
robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
gazeRF.configure(0,NULL);
double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();
ResourceFinder eyeAlignRF;
Bottle &camerasGroup = gazeRF.findGroup("cameras");
if(!camerasGroup.isNull())
{
eyeAlignRF.setVerbose(verbosity!=0);
camerasGroup.check("context")?
eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
eyeAlignRF.configure(0,NULL);
}
else
{
yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");
}
//******************************************************
//*********************** THREAD **********************
if( filenames.size() > 0 )
{
vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
filenames, head_version, eyeAlignRF);
if (!vtRFThrd -> start())
{
delete vtRFThrd;
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated!!");
return false;
}
yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
}
else {
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated. No filenames have been given!");
return false;
}
//******************************************************
//************************ PORTS ***********************
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
void LoggerEngine::discover (std::list<std::string>& ports)
{
RpcClient p;
string logger_portname = log_updater->getPortName();
p.open(logger_portname+"/discover");
std::string yarpservername = yarp::os::Network::getNameServerName();
yarp::os::Network::connect(logger_portname+"/discover",yarpservername.c_str());
Bottle cmd,response;
cmd.addString("bot");
cmd.addString("list");
p.write(cmd,response);
printf ("%s\n\n", response.toString().c_str());
int size = response.size();
for (int i=1; i<size; i++) //beware: skip i=0 is intentional!
{
Bottle* n1 = response.get(i).asList();
if (n1 && n1->get(0).toString()=="port")
{
Bottle* n2 = n1->get(1).asList();
if (n2 && n2->get(0).toString()=="name")
{
char* log_off = nullptr;
char* yarprun_log_off = nullptr;
log_off = std::strstr((char*)(n2->get(1).toString().c_str()), "/log/");
if (log_off)
{
std::string logport = n2->get(1).toString();
printf ("%s\n", logport.c_str());
ports.push_back(logport);
}
yarprun_log_off = std::strstr((char*)(n2->get(1).toString().c_str()), "/yarprunlog/");
if (yarprun_log_off)
{
std::string logport = n2->get(1).toString();
printf ("%s\n", logport.c_str());
ports.push_back(logport);
}
}
}
}
std::list<std::string>::iterator ports_it;
for (ports_it=ports.begin(); ports_it!=ports.end(); ports_it++)
{
LogEntry entry;
entry.logInfo.port_complete = (*ports_it);
yarp::os::Contact contact = yarp::os::Network::queryName(entry.logInfo.port_complete);
if (contact.isValid())
{
entry.logInfo.ip_address = contact.getHost();
}
else
{
printf("ERROR: invalid contact: %s\n", entry.logInfo.port_complete.c_str());
}
std::istringstream iss(*ports_it);
std::string token;
getline(iss, token, '/');
getline(iss, token, '/');
getline(iss, token, '/'); entry.logInfo.port_prefix = "/"+ token;
getline(iss, token, '/'); entry.logInfo.process_name = token;
getline(iss, token, '/'); entry.logInfo.process_pid = token;
std::list<LogEntry>::iterator it;
this->log_updater->mutex.lock();
bool found = false;
for (it = this->log_updater->log_list.begin(); it != log_updater->log_list.end(); it++)
{
if (it->logInfo.port_complete==entry.logInfo.port_complete)
{
found=true; break;
}
}
if (found==false)
{
log_updater->log_list.push_back(entry);
}
this->log_updater->mutex.unlock();
}
}
bool close()
{
port.interrupt();
port.close();
return true;
}
bool configure(ResourceFinder &rf)
{
printf(" Starting Configure\n");
this->rf=&rf;
//****************** PARAMETERS ******************
robot = rf.check("robot",Value("icub")).asString().c_str();
name = rf.check("name",Value("demoINNOROBO")).asString().c_str();
arm = rf.check("arm",Value("right_arm")).asString().c_str();
verbosity = rf.check("verbosity",Value(0)).asInt();
rate = rf.check("rate",Value(0.5)).asDouble();
if (arm!="right_arm" && arm!="left_arm")
{
printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str());
arm="right_arm";
}
printMessage(1,"Parameters correctly acquired\n");
printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str());
printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate);
//****************** DETECTOR ******************
certainty = rf.check("certainty", Value(10.0)).asInt();
strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str();
detectorL=new Detector();
detectorL->open(certainty,strCascade);
detectorR=new Detector();
detectorR->open(certainty,strCascade);
printMessage(1,"Detectors opened\n");
//****************** DRIVERS ******************
Property optionArm("(device remote_controlboard)");
optionArm.put("remote",("/"+robot+"/"+arm).c_str());
optionArm.put("local",("/"+name+"/"+arm).c_str());
if (!drvArm.open(optionArm))
{
printf("Position controller not available!\n");
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote",("/"+robot+"/cartesianController/"+arm).c_str());
optionCart.put("local",("/"+name+"/cart/").c_str());
if (!drvCart.open(optionCart))
{
printf("Cartesian controller not available!\n");
// return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optionGaze))
{
printf("Gaze controller not available!\n");
// return false;
}
// quitting conditions
bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid();
if (!andArm)
{
printMessage(0,"Something wrong occured while configuring drivers... quitting!\n");
return false;
}
drvArm.view(iencs);
drvArm.view(iposs);
drvArm.view(ictrl);
drvArm.view(iimp);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
iimp->setImpedance(0, 0.4, 0.03);
iimp->setImpedance(1, 0.35, 0.03);
iimp->setImpedance(2, 0.35, 0.03);
iimp->setImpedance(3, 0.2, 0.02);
iimp->setImpedance(4, 0.2, 0.00);
ictrl -> setImpedancePositionMode(0);
ictrl -> setImpedancePositionMode(1);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(3);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(4);
igaze -> storeContext(&contextGaze);
igaze -> setSaccadesStatus(false);
igaze -> setNeckTrajTime(0.75);
igaze -> setEyesTrajTime(0.5);
iarm -> storeContext(&contextCart);
printMessage(1,"Drivers opened\n");
//****************** PORTS ******************
imagePortInR -> open(("/"+name+"/imageR:i").c_str());
imagePortInL -> open(("/"+name+"/imageL:i").c_str());
imagePortOutR.open(("/"+name+"/imageR:o").c_str());
imagePortOutL.open(("/"+name+"/imageL:o").c_str());
portOutInfo.open(("/"+name+"/info:o").c_str());
if (robot=="icub")
{
Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str());
Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str());
}
else
{
Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str());
Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str());
}
Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left");
Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right");
Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak");
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
printMessage(0,"Configure Finished!\n");
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("teleop-icub")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str();
double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble();
part=rf.check("part",Value("right_arm")).asString().c_str();
simulator=rf.check("simulator",Value("off")).asString()=="on";
gaze=rf.check("gaze",Value("off")).asString()=="on";
minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble());
maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble());
bool torso=rf.check("torso",Value("on")).asString()=="on";
Property optGeo("(device hapticdeviceclient)");
optGeo.put("remote",("/"+geomagic).c_str());
optGeo.put("local",("/"+name+"/geomagic").c_str());
if (!drvGeomagic.open(optGeo))
return false;
drvGeomagic.view(igeo);
if (simulator)
{
simPort.open(("/"+name+"/simulator:rpc").c_str());
if (!Network::connect(simPort.getName().c_str(),"/icubSim/world"))
{
yError("iCub simulator is not running!");
drvGeomagic.close();
simPort.close();
return false;
}
}
if (gaze)
{
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
{
drvGeomagic.close();
simPort.close();
return false;
}
drvGaze.view(igaze);
}
Property optCart("(device cartesiancontrollerclient)");
optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
optCart.put("local",("/"+name+"/cartesianController/"+part).c_str());
if (!drvCart.open(optCart))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
return false;
}
drvCart.view(iarm);
Property optHand("(device remote_controlboard)");
optHand.put("remote",("/"+robot+"/"+part).c_str());
optHand.put("local",("/"+name+"/"+part).c_str());
if (!drvHand.open(optHand))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
drvCart.close();
return false;
}
drvHand.view(imod);
drvHand.view(ipos);
drvHand.view(ivel);
iarm->storeContext(&startup_context);
iarm->restoreContext(0);
Vector dof(10,1.0);
if (!torso)
dof[0]=dof[1]=dof[2]=0.0;
else
dof[1]=0.0;
iarm->setDOF(dof,dof);
iarm->setTrajTime(Tp2p);
Vector accs,poss;
for (int i=0; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_POSITION);
accs.push_back(1e9);
vels.push_back(100.0);
poss.push_back(0.0);
}
poss[0]=20.0;
poss[1]=70.0;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
ipos->positionMove(joints.size(),joints.getFirst(),poss.data());
joints.clear();
modes.clear();
vels.clear();
for (int i=2; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_VELOCITY);
vels.push_back(40.0);
}
vels[vels.length()-1]=100.0;
s0=s1=idle;
c0=c1=0;
onlyXYZ=true;
stateStr[idle]="idle";
stateStr[triggered]="triggered";
stateStr[running]="running";
Matrix T=zeros(4,4);
T(0,1)=1.0;
T(1,2)=1.0;
T(2,0)=1.0;
T(3,3)=1.0;
igeo->setTransformation(SE3inv(T));
igeo->setCartesianForceMode();
igeo->getMaxFeedback(maxFeedback);
Tsim=zeros(4,4);
Tsim(0,1)=-1.0;
Tsim(1,2)=1.0; Tsim(1,3)=0.5976;
Tsim(2,0)=-1.0; Tsim(2,3)=-0.026;
Tsim(3,3)=1.0;
pos0.resize(3,0.0);
rpy0.resize(3,0.0);
x0.resize(3,0.0);
o0.resize(4,0.0);
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("ssph");
// radius
cmd.addDouble(0.02);
// position
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
// color
cmd.addInt(1);
cmd.addInt(0);
cmd.addInt(0);
// collision
cmd.addString("FALSE");
simPort.write(cmd,reply);
}
forceFbPort.open(("/"+name+"/force-feedback:i").c_str());
feedback.resize(3,0.0);
return true;
}
