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 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);
}
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;
}
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);
}
}
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;
}
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 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 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;
}
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 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;
}
bool respond(const Bottle& command, Bottle& reply)
{
if(command.size()==0)
{
reply.addString("No command received.");
return true;
}
switch(command.get(0).asVocab()){
case HOME:
if(command.size()>1)
switch(command.get(1).asVocab()){
case ARM:
int tempCxL,tempCxR;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
reply.addString("Arm home position reached.");
return true;
case TORSO:
exploreTorso(torsoHomePosition);
reply.addString("Torso home position reached.");
return true;
case GAZE:
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazeHomePosition);
igaze->waitMotionDone(0.1,2);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze home position reached.");
return true;
default:
reply.addString("Wrong device for home position.");
return true;
}
else{
int tempCxL,tempCxR,tempCx;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
igaze->lookAtFixationPoint(gazeHomePosition);
exploreTorso(torsoHomePosition);
igaze->waitMotionDone(0.1,2);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
igaze->restoreContext(tempCx);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
igaze->deleteContext(tempCx);
reply.addString("Ok, moving to home position.");
return true;
}
case LOOK_AT:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
cout<<"first"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
cout<<"second"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_3d")==false ||
bReply.get(1).asList()->find("position_3d").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
cout<<"third"<<endl;
Vector gazePosition(3);
gazePosition[0] = bReply.get(1).asList()->find("position_3d").asList()->get(0).asDouble();
gazePosition[1] = bReply.get(1).asList()->find("position_3d").asList()->get(1).asDouble();
gazePosition[2] = bReply.get(1).asList()->find("position_3d").asList()->get(2).asDouble();
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else if(command.size()==4){
Vector gazePosition(3);
int tempCx;
gazePosition[0] = command.get(1).asDouble();
gazePosition[1] = command.get(2).asDouble();
gazePosition[2] = command.get(3).asDouble();
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else{
reply.addString("Wrong number of parameters for lookAt.");
return true;
}
case GET:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_2d_left")==false ||
bReply.get(1).asList()->find("position_2d_left").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
Vector objPosition(4);
objPosition[0] = bReply.get(1).asList()->find("position_2d_left").asList()->get(0).asInt();
objPosition[1] = bReply.get(1).asList()->find("position_2d_left").asList()->get(1).asInt();
objPosition[2] = bReply.get(1).asList()->find("position_2d_left").asList()->get(2).asInt();
objPosition[3] = bReply.get(1).asList()->find("position_2d_left").asList()->get(3).asInt();
reply.addVocab(VOCAB3('a','c','k'));
Bottle &coord = reply.addList();
coord.addInt((int)(objPosition[0]+objPosition[2])/2);
coord.addInt((int)(objPosition[1]+objPosition[3])/2);
return true;
}
else{
reply.addString("Wrong number of parameters for get.");
return true;
}
case TRACK:
if(command.size()==3)
switch(command.get(1).asVocab()){
case ARM:
if (command.get(2).asString() == "on"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(true);
icartRight->setTrackingMode(true);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(false);
icartRight->setTrackingMode(false);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode disabled.");
}
else
reply.addString("Wrong parameter: on/off");
return true;
case GAZE:
if (command.get(2).asString() == "on"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(true);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(false);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode disabled.");
}
else
reply.addString("Wrong parameter for trac: on/off");
return true;
default:
reply.addString("Wrong device for tracking mode.");
return true;
}
else{
reply.addString("Missing parameters for track.");
return true;
}
case BLOCK:
if(command.size()>1)
switch(command.get(1).asVocab()){
case GAZE:
if(command.size()==3){
// int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(command.get(2).asDouble());
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze blocking mode enabled.");
}
else{
//int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(DEFAULT_VERGENCE);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Default vergence set.");
}
return true;
default:
reply.addString("Wrong device for blocking mode.");
return true;
}
else{
reply.addString("Missing parameters for block.");
return true;
}
case GOTO:
if(command.size()==4){
Vector torsoTarget(3);
torsoTarget[0] = command.get(1).asDouble();
torsoTarget[1] = command.get(2).asDouble();
torsoTarget[2] = command.get(3).asDouble();
exploreTorso(torsoTarget);
reply.addString("Torso position reached.");
}
else{
reply.addString("Missing parameters for goto.");
return true;
}
return true;
case RUN:
running = true;
reply.addString("Ready for exploration. next or stop?");
return true;
case NEXT:
if(running){
exploreTorso(waypoints.getRow(index));
index++;
if (index > 2){
running = false;
index = 0;
reply.addString("Waypoint reached. End of waypoints.");
}
else{
reply.addString("Waypoint reached. next or stop?");
}
}
else
reply.addString("Not running.");
return true;
case STOP:
index = 0;
running = false;
reply.addString("Run stopped. Index reset.");
return true;
default:
RFModule::respond(command,reply);
return true;
}
return true;
}
bool updateModule()
{
ImageOf<PixelMono> *imgDispIn=portDispIn.read();
if (imgDispIn==NULL)
return false;
ImageOf<PixelRgb> *imgIn=portImgIn.read();
if (imgIn==NULL)
return false;
LockGuard lg(mutex);
ImageOf<PixelRgb> &imgDispOut=portDispOut.prepare();
imgDispOut.resize(imgDispIn->width(),imgDispIn->height());
cv::Mat imgDispInMat=cv::cvarrToMat((IplImage*)imgDispIn->getIplImage());
cv::Mat imgDispOutMat=cv::cvarrToMat((IplImage*)imgDispOut.getIplImage());
cv::cvtColor(imgDispInMat,imgDispOutMat,CV_GRAY2RGB);
PixelRgb color(255,255,0);
for (size_t i=0; i<floodPoints.size(); i++)
imgDispOut.pixel(floodPoints[i].x,floodPoints[i].y)=color;
vector<Vector> points;
if (contour.size()>0)
{
vector<vector<cv::Point> > contours;
contours.push_back(contour);
cv::drawContours(imgDispOutMat,contours,0,cv::Scalar(255,255,0));
cv::Rect rect=cv::boundingRect(contour);
cv::rectangle(imgDispOutMat,rect,cv::Scalar(255,50,0));
points.clear();
//if (go||flood3d||flood)
// {
Bottle cmd,reply;
// if (go)
// {
cmd.addString("Rect");
cmd.addInt(rect.x); cmd.addInt(rect.y);
cmd.addInt(rect.width); cmd.addInt(rect.height);
cmd.addInt(downsampling);
if (portSFM.write(cmd,reply))
{
int idx=0;
for (int x=rect.x; x<rect.x+rect.width; x+=downsampling)
{
for (int y=rect.y; y<rect.y+rect.height; y+=downsampling)
{
if (cv::pointPolygonTest(contour,cv::Point2f((float)x,(float)y),false)>0.0)
{
Vector point(6,0.0);
point[0]=reply.get(idx+0).asDouble();
point[1]=reply.get(idx+1).asDouble();
point[2]=reply.get(idx+2).asDouble();
if (norm(point)>0.0)
{
PixelRgb px=imgIn->pixel(x,y);
point[3]=px.r;
point[4]=px.g;
point[5]=px.b;
points.push_back(point);
}
}
idx+=3;
}
}
}
// }
if (flood3d)
{
cmd.addString("Flood3D");
cmd.addInt(contour.back().x);
cmd.addInt(contour.back().y);
cmd.addDouble(spatial_distance);
if (portSFM.write(cmd,reply))
{
for (int i=0; i<reply.size(); i+=5)
{
int x=reply.get(i+0).asInt();
int y=reply.get(i+1).asInt();
PixelRgb px=imgIn->pixel(x,y);
Vector point(6,0.0);
point[0]=reply.get(i+2).asDouble();
point[1]=reply.get(i+3).asDouble();
point[2]=reply.get(i+4).asDouble();
point[3]=px.r;
point[4]=px.g;
point[5]=px.b;
points.push_back(point);
floodPoints.push_back(cv::Point(x,y));
}
}
}
else if (flood)
{
cv::Point seed(contour.back().x,contour.back().y);
PixelMono c=imgDispIn->pixel(seed.x,seed.y);
cv::Scalar delta(color_distance);
cv::floodFill(imgDispInMat,seed,cv::Scalar(255),NULL,delta,delta,4|cv::FLOODFILL_FIXED_RANGE);
cv::cvtColor(imgDispInMat,imgDispOutMat,CV_GRAY2RGB);
}
/** if (points.size()>0)
{
ofstream fout;
fout.open((homeContextPath+"/test-3d-points.off").c_str());
if (fout.is_open())
{
fout<<"COFF"<<endl;
fout<<points.size()<<" 0 0"<<endl;
fout<<endl;
for (size_t i=0; i<points.size(); i++)
{
fout<<points[i].subVector(0,2).toString(3,4).c_str()<<" "<<
points[i].subVector(3,5).toString(0,3).c_str()<<endl;
}
fout<<endl;
}
fout.close();
}*/
go=flood3d=false;
}
// }
Vector p;
p.resize(3,0.0);
for (size_t i=0; i<points.size(); i++)
{
p=points[i];
Bottle &point_out=portOutPoints.prepare();
Bottle &point=point_out.addList();
for(size_t j=0; j<3; j++)
point.addDouble(p[j]);
}
portOutPoints.write();
points.clear();
portDispOut.write();
return true;
}
