Bottle Action::asBottle()
{
Bottle b = this->Entity::asBottle();
Bottle bSub;
bSub.addString("description");
bSub.addList() = initialDescription.asBottle();
b.addList() = bSub;
bSub.clear();
bSub.addString("subactions");
Bottle& subs = bSub.addList();
for(list<Action>::iterator sIt = subActions.begin() ; sIt != subActions.end(); sIt++)
{
subs.addList()=sIt->asBottle();
}
b.addList() = bSub;
bSub.clear();
bSub.addString("estimatedDriveEffects");
Bottle &subss = bSub.addList();
for(map<string, double>::iterator sIt = estimatedDriveEffects.begin() ; sIt != estimatedDriveEffects.end(); sIt++)
{
Bottle &ss = subss.addList();
ss.addString(sIt->first.c_str());
ss.addDouble(sIt->second);
}
b.addList() = bSub;
return b;
}
bool utManagerThread::getPointFromStereo()
{
Bottle cmdSFM;
Bottle respSFM;
cmdSFM.clear();
respSFM.clear();
cmdSFM.addString("Root");
cmdSFM.addInt(int(templatePFTrackerPos(0)));
cmdSFM.addInt(int(templatePFTrackerPos(1)));
SFMrpcPort.write(cmdSFM, respSFM);
// Read the 3D coords and compute the distance to the set reference frame origin
if (respSFM.size() == 3)
{
Vector SFMtmp(3,0.0);
SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate
if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
{
return false;
}
SFMPos = SFMtmp;
return true;
}
return false;
}
void CB::YARPConfigurationVariables::setVelocityControlMode(bool mode, string portName) {
bool ok = true;
// specify the local port names that connect to the velocityControl module
string velocityOutputPortName = "/cb/configuration" + deviceName + "/vel:o";
string velocityRPCOutputPortName = "/cb/configuration" + deviceName + "/vel/rpc:o";
// velocityPortName = portName + "/command";
velocityPortName = portName + "/fastCommand";
velocityRPCPortName = portName + "/input";
Bottle b;
if(mode && !velocityControlMode) {
// if we're turning on the velocityControlMode (and it wasnt previously on)
// open and connect the data and config portsport
ok &= velocityPort.open(velocityOutputPortName.c_str());
ok &= Network::connect(velocityOutputPortName.c_str(),velocityPortName.c_str(),"tcp");
Time::delay(0.5);
ok &= velocityRPCPort.open(velocityRPCOutputPortName.c_str());
ok &= Network::connect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str(),"tcp");
// send gain and maxVel paramaters to the vc module
for(int i=0; i<mask.size(); i++) {
if(mask[i]) {
cout << "setting vc params joint[" << i << "]: gain=" << velocityGain << ", svel=" << maxSetVal << endl;
b.clear();
b.addString("gain");
b.addInt(i);
b.addDouble(velocityGain);
velocityRPCPort.write(b);
Time::delay(0.1);
b.clear();
b.addString("svel");
b.addInt(i);
b.addDouble(maxSetVal);
velocityRPCPort.write(b);
Time::delay(0.1);
}
}
} else if(!mode && velocityControlMode) {
// turning off velocity control mode by disconnecting and closing ports
ok &= Network::disconnect(velocityOutputPortName.c_str(),velocityPortName.c_str());
ok &= Network::disconnect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str());
velocityRPCPort.close();
velocityPort.close();
}
// set the current mode
velocityControlMode = mode;
}
void testSequence(char *seq,
size_t len,
const char *fmt,
Bottle ref,
bool testWrite = true)
{
char err[1024];
printf("\n");
printf("================================================\n");
printf(" READ %s\n", fmt);
Bytes b1(seq, len);
WireTwiddler tt;
tt.configure(fmt, fmt);
printf(">>> %s\n", tt.toString().c_str());
Bottle bot;
checkTrue(tt.read(bot, b1), "Read failed");
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[1] %s: read %s as expected\n", fmt, bot.toString().c_str());
StringInputStream sis;
sis.add(b1);
sis.add(b1);
WireTwiddlerReader twiddled_input(sis, tt);
Route route;
bot.clear();
twiddled_input.reset();
ConnectionReader::readFromStream(bot, twiddled_input);
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str());
bot.clear();
twiddled_input.reset();
ConnectionReader::readFromStream(bot, twiddled_input);
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[3] %s: read %s as expected\n", fmt, bot.toString().c_str());
if (testWrite) {
printf("\n");
printf("================================================\n");
printf(" WRITE %s\n", fmt);
ManagedBytes output;
checkTrue(tt.write(ref, output), "WRITE FAILED");
snprintf(err, 1024, "WRITE MISMATCH, length %zd, expected %zd", output.length(), len);
checkTrue(output.length() == len, err);
for (size_t i = 0; i < output.length(); i++) {
snprintf(err, 1024, "WRITE MISMATCH, at %zd, have [%d:%c] expected [%d:%c]\n", i, output.get()[i], output.get()[i], seq[i], seq[i]);
checkTrue(output.get()[i] == seq[i], err);
}
printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str());
}
}
bool demoModule::respond(const Bottle& command, Bottle& reply) {
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set thr <n> ... set the threshold \n" +
"(where <n> is an integer number) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="set") {
if (command.get(1).asString()=="thr") {
thresholdValue = command.get(2).asInt(); // set parameter value
reply.addString("ok");
}
}
return true;
}
bool visualFilterModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"par <n> <d> ... set parameter's value \n" +
"(where <n> and <d> are an integer and a double respectively) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="par") {
int p = command.get(1).asInt();
double v = command.get(2).asDouble();
if(p>0 && p<8 && v>=0){
vfThread->setPar(p,v);
reply.addString("New kernel generated with updated parameters.");
}
else {
reply.addString("Invalid values");
}
}
return true;
}
bool PARTICLEModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n";
reply.clear();
if (command.get(0).asString()=="quit")
{
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help")
{
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="reset")
{
cout << "reset has been asked "<< endl;
particleManager->shouldSend=false;
reply.addString("ok");
}
else
{
cout << "command not known - type help for more info" << endl;
}
return true;
}
// rpcPort commands handler
bool respond(const Bottle & command,
Bottle & reply)
{
// This method is called when a command string is sent via RPC
// Get command string
string receivedCmd = command.get(0).asString().c_str();
reply.clear(); // Clear reply bottle
if (receivedCmd == "help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("help");
reply.addString("quit");
}
else if (receivedCmd == "quit")
{
reply.addString("Quitting.");
return false; //note also this
}
else
reply.addString("Invalid command, type [help] for a list of accepted commands.");
return true;
}
//*************************************************************************************************************************
MotorFrictionExcitationThread::MotorFrictionExcitationThread(string _name, string _robotName, int _period, ParamHelperServer *_ph,
wholeBodyInterface *_wbi, ResourceFinder &rf, ParamHelperClient *_identificationModule)
: RateThread(_period), name(_name), robotName(_robotName), paramHelper(_ph), robot(_wbi), identificationModule(_identificationModule)
{
status = EXCITATION_OFF;
sendCmdToMotors = SEND_COMMANDS_TO_MOTORS;
printCountdown = 0;
freeExcCounter = 0;
contactExcCounter = 0;
fricStdDevThrMonitor = 0.0;
ktStdDevThrMonitor = 0.0;
_n = robot->getDoFs();
isFrictionStdDevBelowThreshold = false;
Bottle reply;
if(!contactExc.readFromConfigFile(rf, reply))
printf("Error while reading contact excitation from config file: \n%s\n", reply.toString().c_str());
printf("Results of contact excitation reading: %s\n", reply.toString().c_str());
printf("Contact excitation value read:\n%s\n", contactExc.toString().c_str());
reply.clear();
if(!freeMotionExc.readFromConfigFile(rf, reply))
printf("Error while reading free motion excitation from config file: \n%s\n", reply.toString().c_str());
printf("Results of free motion excitation reading: %s\n", reply.toString().c_str());
printf("Free motion excitation value read:\n%s\n", freeMotionExc.toString().c_str());
}
bool updateModule()
{
if (imgOutPort.getOutputCount()>0)
{
if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
{
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Vector v(4,0.0); v[3]=1.0;
Vector c=Ha*v;
v=0.0; v[0]=0.05; v[3]=1.0;
Vector x=Ha*v;
v=0.0; v[1]=0.05; v[3]=1.0;
Vector y=Ha*v;
v=0.0; v[2]=0.05; v[3]=1.0;
Vector z=Ha*v;
v=solution; v.push_back(1.0);
Vector t=Ha*v;
Vector pc,px,py,pz,pt;
int camSel=(eye=="left")?0:1;
igaze->get2DPixel(camSel,c,pc);
igaze->get2DPixel(camSel,x,px);
igaze->get2DPixel(camSel,y,py);
igaze->get2DPixel(camSel,z,pz);
igaze->get2DPixel(camSel,t,pt);
CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);
cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);
cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);
tip.clear();
tip.addInt(point_t.x);
tip.addInt(point_t.y);
imgOutPort.prepare()=*pImgBgrIn;
imgOutPort.write();
}
}
return true;
}
Bottle PMPthread::initHead(Bottle angles)
{
Bottle reply;
reply.clear();
string msg;
//check if the connection is established. If not, try to connect to DevDriver input ports
if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: device ports not active\n");
reply.addString("Error: device ports not active");
return reply;
}
if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
reply.addString(msg.c_str());
return reply;
}
}
//cout << "thread: " << angles.toString() << endl;
updateSem.wait();
rpcPort.write(angles,reply);
//cout << "out of thread!!!!!!!" << endl;
updateSem.post();
return reply;
}
bool ImageSource::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set noise <n> ... set the noise level \n" +
"(where <n> is an integer number in the range 0-255) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="set") {
if (command.get(1).asString()=="noise") {
noiseLevel = command.get(2).asInt(); // set parameter value
reply.addString("ok");
}
}
return true;
}
bool threadInit()
{
fprintf(stdout,"init \n");
port_in0.open(string("/simCOM0:i").c_str());
port_in1.open(string("/simCOM1:i").c_str());
port_in2.open(string("/simCOM2:i").c_str());
port_in3.open(string("/simCOM3:i").c_str());
port_in4.open(string("/simCOM4:i").c_str());
port_in5.open(string("/simCOM5:i").c_str());
port_in6.open(string("/simCOM6:i").c_str());
port_in7.open(string("/simCOM7:i").c_str());
port_in8.open(string("/simCOM8:i").c_str());
port_out.open(string("/simCOM:o").c_str());
yarp::os::Network::connect("/wholeBodyDynamics/com:o","/simCOM0:i");
yarp::os::Network::connect("/wholeBodyDynamics/left_leg/com:o","/simCOM1:i");
yarp::os::Network::connect("/wholeBodyDynamics/left_arm/com:o","/simCOM2:i");
yarp::os::Network::connect("/wholeBodyDynamics/right_leg/com:o","/simCOM3:i");
yarp::os::Network::connect("/wholeBodyDynamics/right_arm/com:o","/simCOM4:i");
yarp::os::Network::connect("/wholeBodyDynamics/head/com:o","/simCOM5:i");
yarp::os::Network::connect("/wholeBodyDynamics/torso/com:o","/simCOM6:i");
yarp::os::Network::connect("/wholeBodyDynamics/upper_body/com:o","/simCOM7:i");
yarp::os::Network::connect("/wholeBodyDynamics/lower_body/com:o","/simCOM8:i");
yarp::os::Network::connect("/simCOM:o","/iCubGui/objects");
Bottle a;
a.clear();
a.addString("reset");
port_out.prepare() = a;
port_out.write();
return true;
}
void getObjectList(Bottle &reply)
{
double t0=Time::now();
int max_size=0;
vector<Blob> max_blobs;
while(Time::now()-t0<0.5)
{
mutex.wait();
reply.clear();
int size=0;
for(unsigned int i=0; i<blobs.size(); i++)
if(blobs[i].getBest()!="")
size++;
if(max_size<size)
{
max_size=size;
max_blobs=blobs;
}
mutex.post();
Time::delay(0.05);
}
reply.addInt(max_size);
for(unsigned int i=0; i<max_blobs.size(); i++)
if(max_blobs[i].getBest()!="")
reply.addString(max_blobs[i].getBest().c_str());
}
bool LRH::respond(const Bottle& command, Bottle& reply) {
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"production objectFocus \n" +
"meaning sentence \n" +
"quit \n";
reply.clear();
if (command.get(0).asString() == "quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString() == "help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString() == "production" && command.size() == 2) {
reply.addString("ack");
reply.addString("OK, send the language production");
cout << "command.get(1).asString() : " << command.get(1).asString() << endl;
spatialRelation(command.get(1).asString());
}
else if (command.get(0).asString() == "meaning" && command.size() == 2) {
reply.addString("ack");
reply.addString("OK, send the language comprehension");
sentenceToMeaning(command.get(1).asString());
}
yInfo("sending reply from rpc");
handlerPort.reply(reply);
return true;
}
void skinEventsAggregThread::run()
{
int indexOfBiggestContact = -1;
skinContact biggestContactInSkinPart;
Vector geoCenter(3,0.0), normalDir(3,0.0);
double activation = 0.0;
Bottle & out = skinEvAggregPortOut.prepare(); out.clear();
Bottle b;
b.clear();
ts.update();
skinContactList *scl = skinEventsPortIn.read(false);
if(scl)
{
if(!(scl->empty()))
{
//Probably source of crazy inefficiencies, here just to reach a working state as soon as possible \todo TODO
map<SkinPart, skinContactList> contactsPerSkinPart = scl->splitPerSkinPart();
for(map<SkinPart,skinContactList>::iterator it=contactsPerSkinPart.begin(); it!=contactsPerSkinPart.end(); it++)
{
indexOfBiggestContact = getIndexOfBiggestContactInList(it->second);
if (indexOfBiggestContact != -1)
{
b.clear();
biggestContactInSkinPart = (it->second)[indexOfBiggestContact];
//the output prepared should have identical format to the one prepared in void vtRFThread::manageSkinEvents()
b.addInt(biggestContactInSkinPart.getSkinPart());
vectorIntoBottle(biggestContactInSkinPart.getGeoCenter(),b);
vectorIntoBottle(biggestContactInSkinPart.getNormalDir(),b);
//we add dummy geoCenter and normalDir in Root frame to keep same format as vtRFThread manageSkinEvents
b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
b.addDouble(std::max(1.0,(biggestContactInSkinPart.getPressure()/SKIN_ACTIVATION_MAX))); // % pressure "normalized" with ad hoc constant
b.addString(biggestContactInSkinPart.getSkinPartName()); //this one just for readability
out.addList().read(b);
}
}
skinEvAggregPortOut.setEnvelope(ts);
skinEvAggregPortOut.write(); // send something anyway (if there is no contact the bottle is empty)
}
}
}
bool respond(const Bottle& command, Bottle& reply)
{
fprintf(stdout,"rpc respond\n");
Bottle cmd;
reply.clear();
return true;
}
Bottle PMPthread::initTorso(Bottle angles)
{
Bottle reply;
reply.clear();
string msg;
//check if the connection is established. If not, try to connect to DevDriver input ports
if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: device ports not active\n");
reply.addString("Error: device ports not active");
return reply;
}
if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
reply.addString(msg.c_str());
return reply;
}
}
double t[3] = {0.0, 0.0, 0.0};
// default initialization
if (angles.size() == 1)
{
for (int i=0; i<3; i++) angles.addDouble(t[i]);
}
// else user defined initialization
updateSem.wait();
rpcPort.write(angles,reply);
// update PMP joint angles
if (reply.get(0).asString() == "Done")
{
PMP->q_rightArm(0) = PMP->q_leftArm(0) = angles.get(0).asDouble()*M_PI/180;
PMP->q_rightArm(1) = PMP->q_leftArm(1) = angles.get(1).asDouble()*M_PI/180;
PMP->q_rightArm(2) = PMP->q_leftArm(2) = angles.get(2).asDouble()*M_PI/180;
PMP->Rchain->setAng(PMP->q_rightArm);
PMP->Lchain->setAng(PMP->q_leftArm);
PMP->x_0R = PMP->get_EEPose("right");
PMP->x_0L = PMP->get_EEPose("left");
//update VTGS starting position
VTGS_r->setStartingPoint(PMP->x_0R);
VTGS_l->setStartingPoint(PMP->x_0L);
}
updateSem.post();
return reply;
}
bool respond(const Bottle &command, Bottle &reply)
{
/* This method is called when a command string is sent via RPC */
reply.clear(); // Clear reply bottle
/* Get command string */
string receivedCmd = command.get(0).asString().c_str();
int responseCode; //Will contain Vocab-encoded response
if (receivedCmd == "merge") {
int ok = MergePointclouds();
if (ok>=0) {
responseCode = Vocab::encode("ack");
} else {
fprintf(stdout,"Couldnt merge pointclouds. \n");
responseCode = Vocab::encode("nack");
return false;
}
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "save") {
saving = true;
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "view") {
visualizing = true;
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
}else if (receivedCmd == "help"){
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("merge - Merges all pointclouds on the path folder, or the new ones if 'cloud_merged' already exists.");
reply.addString("view - Activates visualization. (XXX visualizer does not close). Default off");
reply.addString("save - Activates saving the resulting merged point cloud.");
//reply.addString("verbose ON/OFF - Sets active the printouts of the program, for debugging or visualization.");
reply.addString("help - produces this help.");
reply.addString("quit - closes the module.");
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "quit") {
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
closing = true;
return true;
}
reply.addString("Invalid command, type [help] for a list of accepted commands.");
return true;
}
void testClear() {
report(0,"testing clear...");
Bottle b;
b.addString("hello");
checkEqual(1,b.size(),"size ok");
b.clear();
b.addString("there");
checkEqual(1,b.size(),"size ok");
}
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).isInt())
{
if (command.get(0).asInt()==0)
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
inv_dyn->suspend();
inv_dyn->calibrateOffset();
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands:");
reply.addString("calib all");
reply.addString("calib arms");
reply.addString("calib legs");
reply.addString("calib feet");
return true;
}
else if (command.get(0).asString()=="calib")
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
calib_enum calib_code=CALIB_ALL;
if (command.get(1).asString()=="all") calib_code=CALIB_ALL;
else if (command.get(1).asString()=="arms") calib_code=CALIB_ARMS;
else if (command.get(1).asString()=="legs") calib_code=CALIB_LEGS;
else if (command.get(1).asString()=="feet") calib_code=CALIB_FEET;
inv_dyn->suspend();
inv_dyn->calibrateOffset(calib_code);
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
reply.addString("Unknown command");
return true;
}
void remove_cog(string text)
{
Bottle obj;
obj.clear();
obj.addString("delete"); // command to add/update an object
obj.addString(text.c_str());
port_out.prepare() = obj;
port_out.write();
Time::delay(0.1);
}
void utManagerThread::sendData()
{
Bottle b;
b.clear();
for (size_t i = 0; i < 3; i++)
{
b.addDouble(kalOut(i));
}
outPortEvents.write(b);
}
bool SCSPMClassifier::getOPCList(Bottle &names)
{
names.clear();
if (opcPort.getOutputCount()>0)
{
Bottle opcCmd,opcReply,opcReplyProp;
opcCmd.addVocab(Vocab::encode("ask"));
Bottle &content=opcCmd.addList().addList();
content.addString("entity");
content.addString("==");
content.addString("object");
opcPort.write(opcCmd,opcReply);
if (opcReply.size()>1)
{
if (opcReply.get(0).asVocab()==Vocab::encode("ack"))
{
if (Bottle *idField=opcReply.get(1).asList())
{
if (Bottle *idValues=idField->get(1).asList())
{
// cycle over items
for (int i=0; i<idValues->size(); i++)
{
int id=idValues->get(i).asInt();
// get the relevant properties
// [get] (("id" <num>) ("propSet" ("name")))
opcCmd.clear();
opcCmd.addVocab(Vocab::encode("get"));
Bottle &content=opcCmd.addList();
Bottle &list_bid=content.addList();
list_bid.addString("id");
list_bid.addInt(id);
Bottle &list_propSet=content.addList();
list_propSet.addString("propSet");
list_propSet.addList().addString("name");
opcPort.write(opcCmd,opcReplyProp);
// append the name (if any)
if (opcReplyProp.get(0).asVocab()==Vocab::encode("ack"))
if (Bottle *propField=opcReplyProp.get(1).asList())
if (propField->check("name"))
names.addString(propField->find("name").asString().c_str());
}
}
}
}
}
return true;
}
else
return false;
}
void GuiUpdaterModule::deleteObject(const string &opcTag, Object* o)
{
Bottle cmd;
cmd.addString("delete");
cmd.addString(opcTag.c_str());
toGui.write(cmd);
//Delete all the body parts
if (o != NULL && o->entity_type() == EFAA_OPC_ENTITY_AGENT && displaySkeleton)
{
int i = 0;
Agent* a = dynamic_cast<Agent*>(o);
for(map<string,Vector>::iterator part=a->m_body.m_parts.begin();
part != a->m_body.m_parts.end();
part++)
{
ostringstream opcTagPart;
opcTagPart<<o->opc_id() << "_" << i;
cmd.clear();
cmd.addString("delete");
cmd.addString(opcTagPart.str().c_str());
toGui.write(cmd);
i++;
}
}
//delete all the drives
if (o != NULL && o->entity_type() == EFAA_OPC_ENTITY_AGENT)
{
Agent* a = dynamic_cast<Agent*>(o);
for(map<string,Drive>::iterator drive = a->m_drives.begin(); drive != a->m_drives.end(); drive++)
{
ostringstream opcTagDrive;
opcTagDrive<<a->name()<<"_"<<drive->second.name;
cmd.clear();
cmd.addString("delete");
cmd.addString(opcTagDrive.str().c_str());
toGui.write(cmd);
}
}
}
bool TactileLearning::respond(const Bottle& command, Bottle& reply)
{
stringstream temp;
// reply contains a "command not recognized" by default
reply.clear();
TactileLearningCommand com;
Bottle params;
if(command.get(0).isInt())
{
// if first value is int then it is the id of the command
com = (TactileLearningCommand)command.get(0).asInt();
params = command.tail();
}
else if(!identifyCommand(command, com, params))
{
reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
return true;
}
// external declarations
vector<Vector> referenceTrajectory;
vector<Vector> tactileState;
switch(com)
{
case get_optimal_trajectory:
reply.addString("Module should provide the optimal trajectory if computation is finished");
//thread->getState(tactileState);
//addToBottle(reply, tactileState);
return true;
case get_info:
reply.addString("Sorry but there is no info available for the moment!");
return true;
case get_help:
reply.addString("Sorry but there is no help available for the moment!");
return true;
case quit:
reply.addString("quitting...");
return false;
default:
reply.addString("ERROR: This command is known but it is not managed in the code.");
return true;
}
return true;
}
void AgentDetector::setIdentity(Player p, string name)
{
if (useFaceRecognition)
{
Bottle bFeed;
bFeed.clear();
bFeed.addString("acquire");
bFeed.addString(name.c_str());
faceRecognizerModule.write(bFeed);
}
this->skeletonPatterns[name.c_str()] = getSkeletonPattern(p);
}
void StdoutCallback::onRead(Bottle &bot, const yarp::os::TypedReader<yarp::os::Bottle> &reader){
_mutex.wait();
for(int i = 0; i < bot.size(); i++){
if(bot.get(i).isString()){
_vecStdout.push_back(std::string(bot.get(i).asString().c_str()));
}
}
bot.clear();
while(_vecStdout.size() > 3000){
_vecStdout.pop_back();
}
_mutex.post();
}
int Manager::processHumanCmd(const Bottle &cmd, Bottle &b)
{
int ret=Vocab::encode(cmd.get(0).asString().c_str());
b.clear();
if (cmd.size()>1)
{
if (cmd.get(1).isList())
b=*cmd.get(1).asList();
else
b=cmd.tail();
}
return ret;
}
string PMPthread::PropertyGroup2String(Bottle group)
{
Bottle bot;
bot.clear();
for (int i = 1; i < group.size(); i++)
{
bot.add(group.get(i));
}
string s = bot.toString().c_str();
return s;
}
