bool respond(const Bottle& command, Bottle& reply) {
//handle information requests
string msg(command.get(0).asString().c_str());
if (msg == "rate") {
if (command.size() < 2) {
reply.add(-1);
}
else {
int newrate = command.get(1).asInt();
bool rssuc;
if (newrate > 0) {
rssuc = thr->setRate(newrate);
if (rssuc)
reply.add(1);
else
reply.add(-1);
}
}
}
else {
reply.add(-1);
}
return true;
}
int main(int argc, char *argv[]) {
if (argc<=1) {
printf("This is a very simple database\n");
printf("Call as: %s --name /database\n", argv[0]);
printf("Then you can test it by running:\n");
printf(" yarp rpc /database\n");
printf("And typing things like:\n");
printf(" set x 24\n");
printf(" get x\n");
printf(" get y\n");
printf(" rm x\n");
printf(" get x\n");
printf(" set \"my favorite numbers\" (5 10 16)\n");
printf(" get \"my favorite numbers\"\n");
}
Network yarp;
Property option;
option.fromCommand(argc,argv);
Property state;
Port port;
port.open(option.check("name",Value("/database")).asString());
while (true) {
Bottle cmd;
Bottle response;
port.read(cmd,true); // true -> will reply
Bottle tmp;
tmp.add(cmd.get(1));
std::string key = tmp.toString();
switch (Vocab::encode(cmd.get(0).toString())) {
case VOCAB_SET:
state.put(key,cmd.get(2));
break;
case VOCAB_GET:
break;
case VOCAB_REMOVE:
state.unput(key);
break;
}
Value& v = state.find(key);
response.addVocab(v.isNull()?VOCAB_NOT:VOCAB_IS);
response.add(cmd.get(1));
if (!v.isNull()) {
response.add(v);
}
port.reply(response);
}
return 0;
}
Bottle AdvancedDirectionalSalience::getDebugImageArrayNames(){
Bottle bottle;
for(std::map<string, IplImage***>::const_iterator i = _mapImgTable.begin(); i != _mapImgTable.end(); i++){
bottle.add(Value(i->first.c_str()));
}
return bottle;
}
bool RFModuleHelper::read(ConnectionReader& connection) {
Bottle cmd, response;
if (!cmd.read(connection)) { return false; }
printf("command received: %s\n", cmd.toString().c_str());
bool result = owner.safeRespond(cmd,response);
if (response.size()>=1) {
ConnectionWriter *writer = connection.getWriter();
if (writer!=0) {
if (response.get(0).toString()=="many" && writer->isTextMode()) {
for (int i=1; i<response.size(); i++) {
Value& v = response.get(i);
if (v.isList()) {
v.asList()->write(*writer);
} else {
Bottle b;
b.add(v);
b.write(*writer);
}
}
} else {
response.write(*writer);
}
//printf("response sent: %s\n", response.toString().c_str());
}
}
return result;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool iCub::skinForceControl::identifyCommand(const Bottle &commandBot, const string *cmdList, unsigned int cmdListSize, unsigned int &cmdId, Bottle ¶ms)
{
for(unsigned int i=0; i<cmdListSize; i++){
stringstream stream(cmdList[i]);
string word;
int wordCounter=0;
bool found = true;
while(stream>>word){
if (commandBot.get(wordCounter).asString() != word.c_str()){
found=false;
break;
}
//printf("%s = %s\n", commandBot.get(wordCounter).asString().c_str(), word.c_str());
wordCounter++;
}
if(found){
cmdId = i;
for(int k=wordCounter; k<commandBot.size(); k++)
params.add(commandBot.get(k));
return true;
}
}
return false;
}
bool DeviceResponder::read(ConnectionReader& connection) {
Bottle cmd, response;
if (!cmd.read(connection)) { return false; }
//printf("command received: %s\n", cmd.toString().c_str());
respond(cmd,response);
if (response.size()>=1) {
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
if (response.get(0).toString()=="many"&&writer->isTextMode()) {
for (int i=1; i<response.size(); i++) {
Value& v = response.get(i);
if (v.isList()) {
v.asList()->write(*writer);
} else {
Bottle b;
b.add(v);
b.write(*writer);
}
}
} else {
response.write(*writer);
}
//printf("response sent: %s\n", response.toString().c_str());
}
} else {
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
response.clear();
response.addVocab(Vocab::encode("nak"));
response.write(*writer);
}
}
return true;
}
void addQueue(Bottle& status) {
status.add(Value::makeVocab("q"));
for (deque<Entry>::iterator it=q.begin(); it!=q.end(); it++) {
string name = (*it).name;
status.addString(name.c_str());
}
}
Bottle& putBottleCompat(const char *key, const Bottle& val) {
if (val.get(1).asString()=="=") {
Bottle b;
b.add(val.get(0));
b.append(val.tail().tail());
return putBottle(key,b);
}
return putBottle(key,val);
}
int main() {
Network yarp;
int ct = 0;
Port p; // Create a port.
p.open("/out"); // Give it a name on the network.
while (true) {
Bottle b; // Make a place to store things.
b.clear();
b.add("hello");
b.add("world");
b.add(ct);
ct++;
p.write(b); // Send the data.
printf("Sent %s\n", b.toString().c_str());
Time::delay(1);
}
return 0;
}
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;
}
bool YarpNameSpace::setProperty(const std::string& name, const std::string& key,
const Value& value) {
Bottle command;
command.addString("bot");
command.addString("set");
command.addString(name);
command.addString(key);
command.add(value);
Bottle reply;
NameClient& nic = HELPER(this);
nic.send(command, reply);
return reply.size()>0;
}
bool DeviceResponder::respond(const Bottle& command, Bottle& reply) {
switch (command.get(0).asVocab()) {
case VOCAB4('h','e','l','p'):
if (examples.size()>=1) {
reply.add(Value::makeVocab("many"));
if (command.get(1).toString()=="more") {
reply.append(details);
} else {
reply.append(examples);
}
return true;
} else {
reply.add("no documentation available");
return false;
}
break;
default:
reply.add("command not recognized");
return false;
}
return false;
}
bool yarp::os::Node::Helper::read(ConnectionReader& reader)
{
if (!reader.isValid()) {
return false;
}
NodeArgs na;
na.request.read(reader);
//printf("NODE API for %s received %s\n",
//name.c_str(),
//na.request.toString().c_str());
std::string key = na.request.get(0).asString();
na.args = na.request.tail().tail();
if (key=="getBusStats") {
getBusStats(na);
} else if (key=="getBusInfo") {
getBusInfo(na);
} else if (key=="getMasterUri") {
getMasterUri(na);
} else if (key=="shutdown") {
shutdown(na);
} else if (key=="getPid") {
getPid(na);
} else if (key=="getSubscriptions") {
getSubscriptions(na);
} else if (key=="getPublications") {
getPublications(na);
} else if (key=="paramUpdate") {
paramUpdate(na);
} else if (key=="publisherUpdate") {
publisherUpdate(na);
} else if (key=="requestTopic") {
requestTopic(na);
} else {
na.error("I have no idea what you are talking about");
}
if (na.should_drop) {
reader.requestDrop(); // ROS likes to close down.
}
if (reader.getWriter()) {
Bottle full;
full.addInt32(na.code);
full.addString(na.msg);
full.add(na.reply);
//printf("NODE %s <<< %s\n",
//name.c_str(),
//full.toString().c_str());
full.write(*reader.getWriter());
}
return true;
}
bool Module::basicRespond(const Bottle& command, Bottle& reply) {
switch (command.get(0).asVocab()) {
case VOCAB3('s','e','t'):
state.put(command.get(1).toString(),command.get(2));
reply.addVocab(Vocab::encode("ack"));
return true;
break;
case VOCAB3('g','e','t'):
reply.add(state.check(command.get(1).toString(),Value(0)));
return true;
break;
case VOCAB4('q','u','i','t'):
case VOCAB4('e','x','i','t'):
case VOCAB3('b','y','e'):
reply.addVocab(Vocab::encode("bye"));
stopFlag = true;
interruptModule();
return true;
default:
reply.add("command not recognized");
return false;
}
return false;
}
bool RFModule::basicRespond(const Bottle& command, Bottle& reply) {
switch (command.get(0).asVocab()) {
case VOCAB4('q','u','i','t'):
case VOCAB4('e','x','i','t'):
case VOCAB3('b','y','e'):
reply.addVocab(Vocab::encode("bye"));
stopModule(false); //calls interruptModule()
// interruptModule();
return true;
default:
reply.add("command not recognized");
return false;
}
return false;
}
void testBlob() {
report(0,"testing blob...");
Bottle bot("{4 42 255} 10 20");
checkEqual(bot.size(),3,"plausible parse");
checkTrue(bot.get(0).isBlob(),"blob present");
checkEqual((int)bot.get(0).asBlobLength(),3,"blob length");
checkEqual(bot.get(0).asBlob()[1],42, "blob match");
report(0,"testing blob with internal null...");
char blob[12]="hello\0world";
const Value v ((void*)blob, sizeof(blob));
checkEqual(12,(int)v.asBlobLength(),"value length");
checkFalse(v.isNull(),"value non-null");
Bottle b;
b.add(v);
checkEqual(b.size(),1,"insertion happened");
checkTrue(b.get(0).isBlob(),"insertion is right type");
checkEqual(12,(int)b.get(0).asBlobLength(),"length within bottle");
}
Bottle IOL2OPCBridge::skimBlobs(const Bottle &blobs)
{
Bottle skimmedBlobs;
for (int i=0; i<blobs.size(); i++)
{
CvPoint cog=getBlobCOG(blobs,i);
if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
continue;
// skim out blobs that are too far in the cartesian space
Vector x;
if (get3DPosition(cog,x))
{
if ((x[0]>skim_blobs_x_bounds[0]) && (x[0]<skim_blobs_x_bounds[1]) &&
(x[1]>skim_blobs_y_bounds[0]) && (x[1]<skim_blobs_y_bounds[1]))
skimmedBlobs.add(blobs.get(i));
}
}
return skimmedBlobs;
}
/**
* Identify the command in the bottle and return the correspondent enum value.
* All the elements of the Bottle that are after the identified command are inserted
* in the param bottle.
*/
bool identifyCommand(const Bottle &commandBot, TorqueCtrlTestCommand &com, Bottle ¶m){
for(unsigned int i=0; i<SFC_COMMAND_COUNT; i++){
stringstream stream(TorqueCtrlTestCommand_s[i]);
string word;
int wordCounter=0;
bool found = true;
while(stream>>word){
if (commandBot.get(wordCounter).asString() != word.c_str()){
found=false;
break;
}
wordCounter++;
}
if(found){
com = (TorqueCtrlTestCommand)i;
for(int k=wordCounter; k<commandBot.size(); k++)
param.add(commandBot.get(k));
return true;
}
}
return false;
}
bool selectiveAttentionModule::respond(const Bottle &command,Bottle &reply){
bool ok = false;
bool rec = false; // is the command recognized?
ConstString str=command.get(0).asString();
if(!strcmp(str.c_str(),"sus")) {
currentProcessor->suspend();
return true;
}
else if(!strcmp(str.c_str(),"res")) {
currentProcessor->resume();
return true;
}
else if(!strcmp(str.c_str(),"set")) {
ConstString str2=command.get(0).asString();
if(!strcmp(str2.c_str(),"def")) {
if(currentProcessor!=0) {
currentProcessor->setKMotion(0.2);
currentProcessor->setK1(0.5);
currentProcessor->setK2(0.1);
currentProcessor->setK3(0.5);
currentProcessor->setK4(0.1);
currentProcessor->setK5(0.5);
currentProcessor->setK6(1.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
return true;
}
else if(!strcmp(str2.c_str(),"mot")) {
currentProcessor->setKMotion(1.0);
currentProcessor->setK1(0.0);
currentProcessor->setK2(0.0);
currentProcessor->setK3(0.0);
currentProcessor->setK4(0.0);
currentProcessor->setK5(0.0);
currentProcessor->setK6(0.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
return true;
}
}
}
mutex.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
reply.addVocab(Vocab::encode("many"));
reply.addString("help");
reply.addString("");
reply.addString("set fn \t: general set command ");
reply.addString("");
reply.addString("");
reply.addString("");
reply.addString("set time <int> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("set k1 <double> \t: setting the coefficients to the default value ");
reply.addString("set k1 <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("set k2 <double> \t: setting of linear combination coefficient (map2) ");
reply.addString("set k3 <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("set k4 <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("set k5 <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("set k6 <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("set kc1 <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("set kc2 <double> \t: setting of linear combination coefficient (mapc2) ");
reply.addString("set kmot <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set bu <double> \t: setting of weight of the bottom-up contribution ");
reply.addString("set td <double> \t: setting of weight of the top-down contribution ");
reply.addString("");
reply.addString("get fn \t: general get command ");
reply.addString("");
reply.addString("");
reply.addString("get time <int> \t: getting the timing between saccadic events ");
reply.addString("get k1 <double> \t: getting the coefficients to the default value ");
reply.addString("get k1 <double> \t: getting of linear combination coefficient (map1) ");
reply.addString("get k2 <double> \t: getting of linear combination coefficient (map2) ");
reply.addString("get k3 <double> \t: getting of linear combination coefficient (map3) ");
reply.addString("get k4 <double> \t: getting of linear combination coefficient (map4) ");
reply.addString("get k5 <double> \t: getting of linear combination coefficient (map5) ");
reply.addString("get k6 <double> \t: getting of linear combination coefficient (map6) ");
reply.addString("get kc1 <double> \t: getting of linear combination coefficient (mapc1) ");
reply.addString("get kc2 <double> \t: getting of linear combination coefficient (mapc2) ");
reply.addString("get kmot <double> \t: getting of linear combination coefficient (flow motion) ");
reply.addString(" ");
reply.addString(" ");
ok = true;
}
break;
case COMMAND_VOCAB_SUSPEND:
rec = true;
{
currentProcessor->suspend();
ok = true;
}
break;
case COMMAND_VOCAB_RESUME:
rec = true;
{
currentProcessor->resume();
ok = true;
}
break;
case COMMAND_VOCAB_INH:
rec = true;
{
printf("Inhibition of feature extraction activated \n");
//currentProcessor->suspend();
currentProcessor->magnoCellularSuppression(true);
ok = true;
}
break;
case COMMAND_VOCAB_NINH:
rec = true;
{
printf("Inhibition of feature extraction deactivated \n");
//currentProcessor->resume();
currentProcessor->magnoCellularSuppression(false);
//currentProcessor->setCounterMotion(0);
ok = true;
}
break;
case COMMAND_VOCAB_NAME:
rec = true;
{
// check and change filter name to pass on to the next filter
string fName(command.get(1).asString());
string subName;
Bottle subCommand;
int pos=1;
//int pos = fName.find_first_of(filter->getFilterName());
if (pos == 0){
pos = fName.find_first_of('.');
if (pos > -1){ // there is a subfilter name
subName = fName.substr(pos + 1, fName.size()-1);
subCommand.add(command.get(0));
subCommand.add(Value(subName.c_str()));
}
for (int i = 2; i < command.size(); i++)
subCommand.add(command.get(i));
//ok = filter->respond(subCommand, reply);
}
else{
printf("filter name does not match top filter name ");
ok = false;
}
}
break;
case COMMAND_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SALIENCE_THRESHOLD:{
double thr = command.get(2).asDouble();
}
break;
case COMMAND_VOCAB_NUM_BLUR_PASSES:{
int nb = command.get(2).asInt();
//reply.addString("connection 2");
ok=true;
}
break;
case COMMAND_VOCAB_LHAND:{
int nb = command.get(2).asInt();
if(nb)
currentProcessor->setHandFixation(true);
else
currentProcessor->setHandFixation(false);
ok=true;
}
break;
case COMMAND_VOCAB_TIME:{
int w = command.get(2).asInt();
if(currentProcessor!=0)
currentProcessor->setSaccadicInterval(w);
ok = true;
}
break;
case COMMAND_VOCAB_K1:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK1(w);
ok = true;
}
break;
case COMMAND_VOCAB_K2:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK2(w);
ok = true;
}
break;
case COMMAND_VOCAB_K3:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK3(w);
ok = true;
}
break;
case COMMAND_VOCAB_K4:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK4(w);
ok = true;
}
break;
case COMMAND_VOCAB_K5:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK5(w);
ok = true;
}
break;
case COMMAND_VOCAB_K6:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setK6(w);
ok = true;
}
break;
case COMMAND_VOCAB_BU:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setBU(w);
ok = true;
}
break;
case COMMAND_VOCAB_TD:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setTD(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC1:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKC1(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC2:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKC2(w);
ok = true;
}
break;
case COMMAND_VOCAB_DEF:{
if(currentProcessor!=0) {
currentProcessor->setKMotion(0.2);
currentProcessor->setK1(0.5);
currentProcessor->setK2(0.1);
currentProcessor->setK3(0.5);
currentProcessor->setK4(0.1);
currentProcessor->setK5(0.5);
currentProcessor->setK6(0.1);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
}
ok = true;
}
break;
case COMMAND_VOCAB_KMOT:{
double w = command.get(2).asDouble();
if(currentProcessor!=0)
currentProcessor->setKMotion(w);
ok = true;
}
break;
case COMMAND_VOCAB_MOT:{
if(currentProcessor!=0) {
currentProcessor->setKMotion(1.0);
currentProcessor->setK1(0.0);
currentProcessor->setK2(0.0);
currentProcessor->setK3(0.0);
currentProcessor->setK4(0.0);
currentProcessor->setK5(0.0);
currentProcessor->setK6(0.0);
currentProcessor->setKC1(0.0);
currentProcessor->setKC2(0.0);
}
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(command.get(3).asString().c_str());
}
break;
case COMMAND_VOCAB_WEIGHT:{
double w = command.get(2).asDouble();
}
break;
case COMMAND_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = command.get(3).asDouble();
}
break;
case COMMAND_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
for (int i = 2; i < command.size(); i++)
weights.addDouble(command.get(i).asDouble());
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_GET:
rec = true;
{
reply.addVocab(COMMAND_VOCAB_IS);
reply.add(command.get(1));
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SALIENCE_THRESHOLD:{
double thr=0.0;
reply.addDouble(thr);
ok = true;
}
break;
case COMMAND_VOCAB_NUM_BLUR_PASSES:{
int nb = 0;
reply.addInt(nb);
ok = true;
}
break;
case COMMAND_VOCAB_NAME:{
string s(" ");
reply.addString(s.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(" ");
reply.addString(s.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_COUNT:{
int count =0;
reply.addInt(count);
ok = true;
}
break;
case COMMAND_VOCAB_WEIGHT:{
double w = 0.0;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = 0.0;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
//ok = filter->getChildWeights(&weights);
for (int k = 0; k < weights.size(); k++)
reply.addDouble(0.0);
}
break;
case COMMAND_VOCAB_TIME:{
int w = currentProcessor->getSaccadicInterval();
reply.addInt(w);
ok = true;
}
break;
case COMMAND_VOCAB_KMOT:{
double w = currentProcessor->getKMotion();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K1:{
double w = currentProcessor->getK1();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K2:{
double w = currentProcessor->getK2();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K3:{
double w = currentProcessor->getK3();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K4:{
double w = currentProcessor->getK4();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K5:{
double w = currentProcessor->getK5();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_K6:{
double w = currentProcessor->getK6();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC1:{
double w = currentProcessor->getKC1();
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC2:{
double w = currentProcessor->getKC2();
reply.addDouble(w);
ok = true;
}
/*
break;
case COMMAND_VOCAB_KC3:{
double w = currentProcessor->kc3;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC4:{
double w = currentProcessor->kc4;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC5:{
double w = currentProcessor->kc5;
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_KC6:{
double w = currentProcessor->kc6;
reply.addDouble(w);
ok = true;
}
break;
*/
default: {
}
break;
}
}
break;
}
mutex.post();
if (!rec)
ok = RFModule::respond(command,reply);
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok;
}
bool asvGrabberModule::respond(const Bottle& command, Bottle& reply) {
bool ok = false;
bool rec = false; // is the command recognized?
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");
}
mutex.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
reply.addString("many");
reply.addString("help");
reply.addString("");
reply.addString("set fn \t: general set command ");
reply.addString("");
reply.addString("");
reply.addString("");
reply.addString("set left pr <int> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("set left foll <double> \t: setting the coefficients to the default value ");
reply.addString("set left diff <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("set left diffon<double> \t: setting of linear combination coefficient (map2) ");
reply.addString("set left puy <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("set left refr <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("set left req <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("set left diffoff <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("set left pux <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("set left reqpd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set left injgnd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set left cas <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("");
reply.addString("get fn \t: general get command ");
reply.addString("");
reply.addString("");
reply.addString("get left pr <double> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("get left foll <double> \t: setting the coefficients to the default value ");
reply.addString("get left diff <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("get left diffon<double> \t: setting of linear combination coefficient (map2) ");
reply.addString("get left puy <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("get left refr <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("get left req <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("get left diffoff <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("get left pux <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("get left reqpd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("get left injgnd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("get left cas <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("");
reply.addString("");
reply.addString("prog fn \t: general prog command ");
reply.addString("");
reply.addString("");
reply.addString("prog left bias \t: asks to reprogram biases ");
reply.addString("prog right bias \t: asks to reprogram biases ");
reply.addString("dump on \t: asks to reprogram biases ");
reply.addString("dump off \t: asks to reprogram biases ");
ok = true;
}
break;
case COMMAND_VOCAB_SUSPEND:
rec = true;
{
D2Y->suspend();
ok = true;
}
break;
case COMMAND_VOCAB_RESUME:
rec = true;
{
D2Y->resume();
ok = true;
}
break;
case COMMAND_VOCAB_NAME:
rec = true;
{
// check and change filter name to pass on to the next filter
string fName(command.get(1).asString());
string subName;
Bottle subCommand;
int pos=1;
//int pos = fName.find_first_of(filter->getFilterName());
if (pos == 0) {
pos = fName.find_first_of('.');
if (pos > -1) { // there is a subfilter name
subName = fName.substr(pos + 1, fName.size()-1);
subCommand.add(command.get(0));
subCommand.add(Value(subName.c_str()));
}
for (int i = 2; i < command.size(); i++)
subCommand.add(command.get(i));
//ok = filter->respond(subCommand, reply);
}
else {
printf("filter name does not match top filter name ");
ok = false;
}
}
break;
case COMMAND_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SYTH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYTA: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynTau(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPA: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlTau(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_TPB: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setTestPbias(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDR: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDRefr(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDS: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDSf(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDP: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDPr(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPX: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setReqPuX(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPY: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setReqPuY(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFR: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFRf(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFT: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFLk(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDOF: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDOffThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPW: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlW(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYW: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynW(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDON: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDOnThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDD: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDDiff(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMCompH(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCT: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMCompT(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDI: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDIoff(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDRG: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDRGnd(w);
ok = true;
}
break;
case COMMAND_VOCAB_SELF: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSelf(w);
ok = true;
}
break;
case COMMAND_VOCAB_FOLL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setFollBias(w);
ok = true;
}
break;
case COMMAND_VOCAB_ARBP: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setArbPd(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMVrefL(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDC: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDCas(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMVrefH(w);
ok = true;
}
break;
default: {
} break;
} // closing the inner switch
}
break; //closing the SET
case COMMAND_VOCAB_GET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SYTH: {
double w = D2Y->getSynThr();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYTA: {
double w = D2Y->getSynTau();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPA: {
double w = D2Y->getSynPxlTau();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPH: {
double w = D2Y->getSynPxlThr();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_TPB: {
double w = D2Y->getTestPBias();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_CDR: {
double w = D2Y->getCDRefr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDS: {
double w = D2Y->getCDSf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDP: {
double w = D2Y->getCDPr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPX: {
double w = D2Y->getReqPuX();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPY : {
double w = D2Y->getReqPuY();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFR : {
double w = D2Y->getIFRf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFT : {
double w = D2Y->getIFThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFL : {
double w = D2Y->getIFLk();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDOF: {
double w = D2Y->getCDOffThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPW: {
double w = D2Y->getSynPxlW();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYW: {
double w = D2Y->getSynW();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDON: {
double w = D2Y->getCDOnThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDD: {
double w = D2Y->getCDDiff();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCH: {
double w = D2Y->getEMCompH();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCT: {
double w = D2Y->getEMCompT();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDI: {
double w = D2Y->getCDIoff();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDRG: {
double w = D2Y->getCDRGnd();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SELF: {
double w = D2Y->getSelf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_FOLL: {
double w = D2Y->getFollBias();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_ARBP: {
double w = D2Y->getArbPd();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVL: {
double w = D2Y->getEMVrefL();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDC : {
double w = D2Y->getCDCas();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVH: {
double w = D2Y->getEMVrefH();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_PROG:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_LEFT: {
printf("request of reprogrammming biases arrived \n");
D2Y->closeDevice();
Time::delay(1);
D2Y->setFromBinary(false);
D2Y->prepareBiases();
ok = true;
}
break;
case COMMAND_VOCAB_RIGHT: {
printf("request of reprogrammming biases arrived \n");
D2Y->closeDevice();
Time::delay(1);
D2Y->setFromBinary(false);
D2Y->prepareBiasesRight();
ok = true;
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_DUMP:
rec = true;
printf("recognised the command DUMP \n");
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_ON: {
printf("request of start dumping events arrived \n");
string filename = (string) command.get(2).asString();
if(strcmp(filename.c_str(),"")) {
D2Y->setDumpFile(filename);
D2Y->setDumpEvent(true);
printf("success in opening the dump file \n");
}
ok = true;
}
break;
case COMMAND_VOCAB_OFF: {
printf("request of stop dumping events arrived \n");
D2Y->setDumpEvent(false);
ok = true;
}
break;
}
}
break;
} //end of the outer switch
mutex.post();
if (!rec) {
ok = RFModule::respond(command,reply);
}
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok;
//return true;
}
virtual void run(){
tmp = command;
(*command)[0] = -60*(gsl_rng_uniform(r));
(*command)[1] = 100*(gsl_rng_uniform(r));
(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
//above 0 doesn't seem to be safe for joint 0
if ((*command)[0] > 0 || (*command)[0] < -60){
(*command)[0] = (*tmp)[0];
}
if ((*command)[1] > 100 || (*command)[1] < -0){
(*command)[1] = (*tmp)[1];
}
if ((*command)[2] > 60 || (*command)[2] < -35){
(*command)[2] = (*tmp)[2];
}
if ((*command)[3] > 100 || (*command)[3] < 10){
(*command)[3] = (*tmp)[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add((*command)[i]);
}
armPlan->write(plan);
armPred->read(pred);
Vector commandCart(3);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
printf("Cartesian safety check\n");
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command->data());
bool done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
printf("Moved arm to new location\n");
Vector &armJ = armLocJ->prepare();
Vector encoders(nj);
enc->getEncoders(encoders.data());
armJ = encoders;
Vector noisyArm(3);
for(int i = 0; i < 3; i++){
//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
//sanity check
noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
}
//insert here:
//read off peak saliences
//fixate there
//calculate cartesian value, compare to real cart. value of arm
printf("Looking at arm\n");
igaze->lookAtFixationPoint(noisyArm);
done = false;
while(!done){
igaze->checkMotionDone(&done);
Time::delay(0.5);
}
//igaze->waitMotionDone(0.1,30);
printf("Saw arm\n");
Vector &headAng = headLoc->prepare();
igaze->getAngles(headAng);
Bottle tStamp;
tStamp.clear();
tStamp.add(Time::now());
headLoc->setEnvelope(tStamp);
headLoc->write();
armLocJ->write();
headLoc->unprepare();
armLocJ->unprepare();
}
else{
printf("Self collision detected!\n");
}
}
bool SalienceModule::respond(const Bottle &command,Bottle &reply){
bool ok = false;
bool rec = false; // is the command recognized?
mutex.wait();
switch (command.get(0).asVocab()) {
case SALIENCE_VOCAB_HELP:
rec = true;
{
reply.addString("help");
reply.addString("\n");
reply.addString("get fn \t: get the name of the top filter in the filter hierarchy");
reply.addString("get nb \t: get the number of blur passes (gaussian 3x3) applied to the final salience map");
reply.addString("get th \t: get the threshold applied to the final salience map");
reply.addString("get cn <i> \t: get the name of the i-th sub-filter");
reply.addString("get cw <i> \t: get the weight of the i-th sub-filter");
reply.addString("get cc \t: get the number of sub-filters");
reply.addString("get cws \t: get the weights of all the sub-filters");
reply.addString("\n");
reply.addString("set fn <s> \t: set the name of the top-filter ((string)s)");
reply.addString("set nb \t: set the number of blur passes (gaussian 3x3) applied to the final salience map (int[>=0])");
reply.addString("set th \t: set the threshold applied to the final salience map (int[0-255])");
reply.addString("set w <d> \t: set the weight of the top-filter");
reply.addString("set cn <i> <s> \t: set the name of the i-th sub-filter ((string)s)");
reply.addString("set cw <i> <d> \t: set the weight of the i-th sub-filter ((double)d[0.0-1.0])");
reply.addString("set cws <d> ... <d> \t: set weights to all the sub-filters ((double)d[0.0-1.0])");
reply.addString("\n");
reply.addString("Experimental Syntax:");
reply.addString("fn <topFilterName>.<subFilterName> set w <double> \t: set the weighting of a filter (double[0.0-1.0])");
reply.addString("fn <topFilterName>.<subFilterName> get w <double> \t: get the weighting of a filter (double[0.0-1.0])");
ok = true;
}
break;
case SALIENCE_VOCAB_NAME:
rec = true;
{
// check and change filter name to pass on to the next filter
string fName(command.get(1).asString());
string subName;
Bottle subCommand;
int pos = fName.find_first_of(filter->getFilterName());
if (pos == 0){
pos = fName.find_first_of('.');
if (pos > -1){ // there is a subfilter name
subName = fName.substr(pos + 1, fName.size()-1);
subCommand.add(command.get(0));
subCommand.add(Value(subName.c_str()));
}
for (int i = 2; i < command.size(); i++)
subCommand.add(command.get(i));
ok = filter->respond(subCommand, reply);
}
else{
cout << "filter name " << fName << " does not match top filter name " << filter->getFilterName() << endl;
ok = false;
}
}
break;
case SALIENCE_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case SALIENCE_VOCAB_SALIENCE_THRESHOLD:{
double thr = command.get(2).asDouble();
ok = this->setSalienceThreshold(thr);
}
break;
case SALIENCE_VOCAB_NUM_BLUR_PASSES:{
int nb = command.get(2).asInt();
ok = this->setNumBlurPasses(nb);
}
break;
/*case SALIENCE_VOCAB_TEMPORAL_BLUR:{
int size = command.get(2).asInt();
ok = this->setTemporalBlur(size);
}*/
break;
case SALIENCE_VOCAB_NAME:{
string s(command.get(2).asString().c_str());
ok = filter->setFilterName(s);
}
break;
case SALIENCE_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(command.get(3).asString().c_str());
ok = filter->setChildFilterName(j,s);
}
break;
case SALIENCE_VOCAB_WEIGHT:{
double w = command.get(2).asDouble();
ok = filter->setWeight(w);
}
break;
case SALIENCE_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = command.get(3).asDouble();
ok = filter->setChildWeight(j,w);
}
break;
case SALIENCE_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
for (int i = 2; i < command.size(); i++)
weights.addDouble(command.get(i).asDouble());
ok = filter->setChildWeights(weights);
}
break;
default:
cout << "received an unknown request after a SALIENCE_VOCAB_SET" << endl;
break;
}
}
break;
case SALIENCE_VOCAB_GET:
rec = true;
{
reply.addVocab(SALIENCE_VOCAB_IS);
reply.add(command.get(1));
switch(command.get(1).asVocab()) {
case SALIENCE_VOCAB_SALIENCE_THRESHOLD:{
double thr = this->getSalienceThreshold();
reply.addDouble(thr);
ok = true;
}
break;
case SALIENCE_VOCAB_NUM_BLUR_PASSES:{
int nb = this->getNumBlurPasses();
reply.addInt(nb);
ok = true;
}
break;
case SALIENCE_VOCAB_NAME:{
string s(filter->getFilterName());
reply.addString(s.c_str());
ok = true;
}
break;
case SALIENCE_VOCAB_CHILD_NAME:{
int j = command.get(2).asInt();
string s(filter->getChildFilterName(j));
reply.addString(s.c_str());
ok = true;
}
break;
case SALIENCE_VOCAB_CHILD_COUNT:{
int count = filter->getChildCount();
reply.addInt(count);
ok = true;
}
break;
case SALIENCE_VOCAB_WEIGHT:{
double w = filter->getWeight();
reply.addDouble(w);
ok = true;
}
break;
case SALIENCE_VOCAB_CHILD_WEIGHT:{
int j = command.get(2).asInt();
double w = filter->getChildWeight(j);
reply.addDouble(w);
ok = true;
}
break;
case SALIENCE_VOCAB_CHILD_WEIGHTS:{
Bottle weights;
ok = filter->getChildWeights(&weights);
for (int k = 0; k < weights.size(); k++)
reply.addDouble(weights.get(k).asDouble());
}
break;
default:
cout << "received an unknown request after a SALIENCE_VOCAB_GET" << endl;
break;
}
}
break;
}
mutex.post();
if (!rec)
ok = RFModule::respond(command,reply);
if (!ok) {
reply.clear();
reply.addVocab(SALIENCE_VOCAB_FAILED);
}
else
reply.addVocab(SALIENCE_VOCAB_OK);
return ok;
}
void YarpPluginSelector::scan() {
config.clear();
plugins.clear();
search_path.clear();
NetworkBase::lock();
Property& state = NameClient::getNameClient().getPluginState();
config = state;
NetworkBase::unlock();
bool need_scan = true;
if (config.check("last_update_time")) {
if (Time::now()-config.find("last_update_time").asDouble()<5) {
need_scan = false;
}
}
if (need_scan) {
YARP_SPRINTF0(Logger::get(),
debug,
"Scanning. I'm scanning. I hope you like scanning too.");
NetworkBase::lock();
ResourceFinder& rf = ResourceFinder::getResourceFinderSingleton();
if (!rf.isConfigured()) {
rf.configure(0,YARP_NULLPTR);
}
rf.setQuiet(true);
Bottle plugins = rf.findPaths("plugins");
if (plugins.size()==0) {
plugins = rf.findPaths("share/yarp/plugins");
}
if (plugins.size()>0) {
for (int i=0; i<plugins.size(); i++) {
ConstString target = plugins.get(i).asString();
YARP_SPRINTF1(Logger::get(),
debug,
"Loading configuration files related to plugins from %s.", target.c_str());
config.fromConfigDir(target, "inifile", false);
}
} else {
YARP_SPRINTF0(Logger::get(),
debug,
"Plugin directory not found");
}
config.put("last_update_time",Time::now());
state = config;
NetworkBase::unlock();
}
Bottle inilst = config.findGroup("inifile").tail();
for (int i=0; i<inilst.size(); i++) {
ConstString inifile = inilst.get(i).asString();
Bottle inigroup = config.findGroup(inifile);
Bottle lst = inigroup.findGroup("plugin").tail();
for (int i=0; i<lst.size(); i++) {
ConstString plugin_name = lst.get(i).asString();
Bottle group = inigroup.findGroup(plugin_name);
group.add(Value::makeValue(ConstString("(inifile \"") + inifile + "\")"));
if (select(group)) {
plugins.addList() = group;
}
}
lst = inigroup.findGroup("search").tail();
for (int i=0; i<lst.size(); i++) {
ConstString search_name = lst.get(i).asString();
Bottle group = inigroup.findGroup(search_name);
search_path.addList() = group;
}
}
}
bool execReq(const Bottle &command, Bottle &reply)
{
switch(command.get(0).asVocab())
{
case(CMD_HELP):
{
reply.clear();
reply.add(Value::makeVocab("many"));
reply.addString("[dump] [path-to-file] [a] to start dumping the codes in the context directory. Use 'a' for appending.");
reply.addString("[stop] to stop dumping.");
return true;
}
case(DUMP_CODE):
{
mutex.wait();
dump_code = true;
string code_path;
string code_write_mode;
if (command.size()==1)
{
code_path = contextPath + "/codes.bin";
code_write_mode="wb";
}
else if (command.size()==2)
{
if (strcmp(command.get(1).asString().c_str(),"a")==0)
{
code_write_mode="wb+";
code_path = contextPath + "/codes.bin";
} else
{
code_write_mode="wb";
code_path = command.get(1).asString().c_str();
}
} else if (command.size()==3)
{
code_write_mode="wb+";
code_path = command.get(2).asString().c_str();
}
fout_code = fopen(code_path.c_str(),code_write_mode.c_str());
reply.addString("Start dumping codes...");
mutex.post();
return true;
}
case(DUMP_STOP):
{
mutex.wait();
dump_code = false;
fclose(fout_code);
reply.addString("Stopped code dump.");
mutex.post();
return true;
}
default:
return false;
}
}
void fromCommand(int argc, char *argv[],bool wipe=true) {
String tag = "";
Bottle accum;
Bottle total;
bool qualified = false;
for (int i=0; i<argc; i++) {
String work = argv[i];
bool isTag = false;
if (work.length()>=2) {
if (work[0]=='-'&&work[1]=='-') {
work = work.substr(2,work.length()-2);
isTag = true;
if (work.find("::")!=String::npos) {
qualified = true;
}
}
}
if (isTag) {
if (tag!="") {
total.addList().copy(accum);
}
tag = work;
accum.clear();
} else {
if (work.find("\\")!=String::npos) {
// Specifically when reading from the command
// line, we will allow windows-style paths.
// Hence we have to break the "\" character
String buf = "";
for (unsigned int i=0; i<work.length(); i++) {
buf += work[i];
if (work[i]=='\\') {
buf += work[i];
}
}
work = buf;
}
}
accum.add(Value::makeValue(work.c_str()));
}
if (tag!="") {
total.addList().copy(accum);
}
if (!qualified) {
fromBottle(total,wipe);
return;
}
if (wipe) {
clear();
}
Bottle *cursor = NULL;
for (int i=0; i<total.size(); i++) {
cursor = NULL;
Bottle *term = total.get(i).asList();
if (!term) continue;
ConstString key = term->get(0).asString();
ConstString base = key;
while (key.length()>0) {
int at = key.find("::");
base = key;
if (at>=0) {
base = key.substr(0,at);
key = key.substr(at+2);
} else {
key = "";
}
Bottle& result = (cursor!=NULL)? (cursor->findGroup(base.c_str())) : owner.findGroup(base.c_str());
if (result.isNull()) {
if (!cursor) {
cursor = &putBottle((base).c_str());
} else {
cursor = &cursor->addList();
}
cursor->addString(base);
} else {
cursor = &result;
}
}
if (cursor) {
cursor->copy(*term);
cursor->get(0) = Value(base);
}
}
}
Value toValue(XmlRpcValue& v, bool outer)
{
int t = v.getType();
switch (t) {
case XmlRpcValue::TypeInt:
return Value((int)v);
break;
case XmlRpcValue::TypeDouble:
return Value((double)v);
break;
case XmlRpcValue::TypeString:
{
string s = (string)v;
if (s.length()==0 || s[0]!='[') {
return Value(s);
} else {
Value v;
v.fromString(s.c_str());
return v;
}
}
break;
case XmlRpcValue::TypeArray:
{
Value vbot;
Bottle *bot = vbot.asList();
for (int i=0; i<v.size(); i++) {
XmlRpcValue& v2 = v[i];
if (v2.getType()!=XmlRpcValue::TypeInvalid) {
Value v = toValue(v2,false);
if (i==0) {
std::string tag = v.asString();
if (tag=="list"||tag=="dict") {
if (!outer) {
bot->addString("list");
}
}
}
bot->add(v);
}
}
return vbot;
}
break;
case XmlRpcValue::TypeStruct:
{
Value vbot;
Bottle *bot = vbot.asList();
XmlRpcValue::ValueStruct& vals = v;
bot->addString("dict");
for (auto& val : vals) {
XmlRpcValue& v2 = val.second;
Bottle& sub = bot->addList();
sub.addString(val.first.c_str());
if (v2.getType()!=XmlRpcValue::TypeInvalid) {
sub.add(toValue(v2,false));
}
}
return vbot;
}
break;
case XmlRpcValue::TypeInvalid:
return Value::getNullValue();
break;
}
//printf("Skipping %d\n", t);
return Value("(type not supported yet out of laziness)");
}
bool AdvancedDirectionalSalience::respond(const Bottle &command,Bottle &reply){
bool ok = false; // command executed successfully
bool rec = false; // command recognized
switch (command.get(0).asVocab()) {
case SALIENCE_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case SALIENCE_VOCAB_DIRECTIONAL_DBG_SCALE_INDEX:{
int index = command.get(2).asInt();
ok = this->setDebugFilterScaleIndex(index);
}
break;
case SALIENCE_VOCAB_DIRECTIONAL_DBG_DIRECTION_INDEX:{
int index = command.get(2).asInt();
ok = this->setDebugFilterDirectionIndex(index);
}
break;
case SALIENCE_VOCAB_DIRECTIONAL_DBG_IMAGE_ARRAY_NAME:{
string name(command.get(2).asString().c_str());
ok = this->setDebugImageArrayName(name);
}
break;
default:
cout << "AdvancedDirectionalSalience::respond(): received an unknown request after a SALIENCE_VOCAB_SET" << endl;
break;
}
}
break;
case SALIENCE_VOCAB_GET:
rec = true;
{
reply.addVocab(SALIENCE_VOCAB_IS);
reply.add(command.get(1));
switch(command.get(1).asVocab()) {
case SALIENCE_VOCAB_DIRECTIONAL_NUM_DIRECTIONS:{
int num = this->getNumberDirections();
reply.addInt(num);
ok = true;
}
break;
case SALIENCE_VOCAB_DIRECTIONAL_NUM_SCALES:{
int num = this->getNumberScales();
reply.addInt(num);
ok = true;
}
break;
case SALIENCE_VOCAB_DIRECTIONAL_DBG_IMAGE_ARRAY_NAMES:{
Bottle &bot = reply.addList();
bot.fromString(this->getDebugImageArrayNames().toString());
ok = true;
}
break;
default:
cout << "AdvancedDirectionalSalience::respond(): received an unknown request after a SALIENCE_VOCAB_GET" << endl;
break;
}
}
break;
}
return ok;
}
bool execReq(const Bottle &command, Bottle &reply)
{
switch(command.get(0).asVocab())
{
case(CMD_HELP):
{
reply.clear();
if(!help)
{
reply.addString("There's no help in this life. You oughta do everything on your own");
help=true;
} else
{
reply.add(Value::makeVocab("many"));
reply.addString("Ok joking.. here the help!");
reply.addString("[dump] [a] for starting the SIFT dumping in the context directory.. use 'a' for appending");
reply.addString("[stop] for stopping the SIFT dumping in the context directory.. ");
reply.addString("[learn] [dictionarySize] [usePCA] [dimPCA] for learning the dictionary from a SIFT file previously dumped..");
}
return true;
}
case(DUMP_SIFT):
{
mutex.wait();
dump_sift=true;
string sift_path="sift.txt";
sift_path=contextPath+"/"+sift_path;
string sift_write_mode;
if(command.size()==1)
sift_write_mode="w";
else
sift_write_mode=command.get(1).asString().c_str();
fout_sift=fopen(sift_path.c_str(),sift_write_mode.c_str());
reply.addString("Starting to dump SIFTs...");
mutex.post();
return true;
}
case(DUMP_STOP):
{
mutex.wait();
dump_sift=false;
fclose(fout_sift);
reply.addString("Stopped SIFT Dump.");
mutex.post();
return true;
}
case(LEARN_DICT):
{
mutex.wait();
//string sift_path="sift.txt";
//sift_path=contextPath+"/"+sift_path;
string sift_path=rf.findFile("sift.txt").c_str();
string dictionary_path="newDictionary.ini";
dictionary_path=contextPath+"/"+dictionary_path;
reply.addString("Dictionary Learned.");
int dictSize;
if(command.size()==1)
dictSize=512;
else
dictSize=command.get(1).asInt();
bool usePCA=command.size()>2;
int dimPCA;
if(command.size()>3)
{
dimPCA=command.get(3).asInt();
}else
{
if(usePCA)
dimPCA=80;
}
sparse_coder->learnDictionary(sift_path, dictSize,usePCA,dimPCA);
sparse_coder->saveDictionary(dictionary_path);
mutex.post();
return true;
}
default:
return false;
}
}
bool add(const Value& v) {
//printf("adding value %s\n", v.toString().c_str());
b.add(v);
return true;
}
