void handleImpedanceMsg(IImpedanceControl *iimp, const yarp::os::Bottle& cmd,
yarp::os::Bottle& response, bool *rec, bool *ok)
{
fprintf(stderr, "Handling IImpedance messages\n");
if (!iimp)
{
fprintf(stderr, "Error, I do not have a valid IImpedance interface\n");
*ok=false;
return;
}
int controlledJoints;
iimp->getAxes(&controlledJoints);
int code = cmd.get(1).asVocab();
switch (code)
{
case VOCAB_SET:
{
*rec = true;
switch(cmd.get(2).asVocab())
{
case VOCAB_IMP_PARAM:
{
*ok = iimp->setImpedance(cmd.get(3).asInt(), cmd.get(4).asDouble(),cmd.get(5).asDouble());
}
break;
case VOCAB_IMP_OFFSET:
{
*ok = iimp->setImpedanceOffset (cmd.get(3).asInt(), cmd.get(4).asDouble());
}
break;
}
}
break;
case VOCAB_GET:
{
*rec = true;
int tmp = 0;
double dtmp0 = 0.0;
double dtmp1 = 0.0;
double dtmp2 = 0.0;
response.addVocab(VOCAB_IS);
response.add(cmd.get(1));
switch(cmd.get(2).asVocab())
{
case VOCAB_IMP_PARAM:
{
*ok = iimp->getImpedance(cmd.get(3).asInt(), &dtmp0, &dtmp1);
response.addDouble(dtmp0);
response.addDouble(dtmp1);
}
break;
case VOCAB_IMP_OFFSET:
{
*ok = iimp->getImpedanceOffset(cmd.get(3).asInt(), &dtmp0);
response.addDouble(dtmp0);
}
break;
}
}
break;
}
//rec --> true se il comando e' riconosciuto
//ok --> contiene il return value della chiamata all'interfaccia
// ...*ok=torque->setPid();
//torque->
}
bool ServerFrameGrabber::respond(const yarp::os::Bottle& cmd,
yarp::os::Bottle& response) {
int code = cmd.get(0).asVocab();
IFrameGrabberControlsDC1394* fgCtrlDC1394=dynamic_cast<IFrameGrabberControlsDC1394*>(fgCtrl);
//printf("%s\n",cmd.toString().c_str());
switch (code)
{
case VOCAB_SET:
printf("set command received\n");
switch(cmd.get(1).asVocab())
{
case VOCAB_BRIGHTNESS:
response.addInt(int(setBrightness(cmd.get(2).asDouble())));
return true;
case VOCAB_EXPOSURE:
response.addInt(int(setExposure(cmd.get(2).asDouble())));
return true;
case VOCAB_SHARPNESS:
response.addInt(int(setSharpness(cmd.get(2).asDouble())));
return true;
case VOCAB_WHITE:
response.addInt(int(setWhiteBalance(cmd.get(2).asDouble(),cmd.get(3).asDouble())));
return true;
case VOCAB_HUE:
response.addInt(int(setHue(cmd.get(2).asDouble())));
return true;
case VOCAB_SATURATION:
response.addInt(int(setSaturation(cmd.get(2).asDouble())));
return true;
case VOCAB_GAMMA:
response.addInt(int(setGamma(cmd.get(2).asDouble())));
return true;
case VOCAB_SHUTTER:
response.addInt(int(setShutter(cmd.get(2).asDouble())));
return true;
case VOCAB_GAIN:
response.addInt(int(setGain(cmd.get(2).asDouble())));
return true;
case VOCAB_IRIS:
response.addInt(int(setIris(cmd.get(2).asDouble())));
return true;
/*
case VOCAB_TEMPERATURE:
response.addInt(int(setTemperature(cmd.get(2).asDouble())));
return true;
case VOCAB_WHITE_SHADING:
response.addInt(int(setWhiteShading(cmd.get(2).asDouble(),cmd.get(3).asDouble(),cmd.get(4).asDouble())));
return true;
case VOCAB_OPTICAL_FILTER:
response.addInt(int(setOpticalFilter(cmd.get(2).asDouble())));
return true;
case VOCAB_CAPTURE_QUALITY:
response.addInt(int(setCaptureQuality(cmd.get(2).asDouble())));
return true;
*/
}
return DeviceResponder::respond(cmd,response);
case VOCAB_GET:
printf("get command received\n");
response.addVocab(VOCAB_IS);
response.add(cmd.get(1));
switch(cmd.get(1).asVocab())
{
case VOCAB_BRIGHTNESS:
response.addDouble(getBrightness());
return true;
case VOCAB_EXPOSURE:
response.addDouble(getExposure());
return true;
case VOCAB_SHARPNESS:
response.addDouble(getSharpness());
return true;
case VOCAB_WHITE:
{
double b=0.0;
double r=0.0;
getWhiteBalance(b,r);
response.addDouble(b);
response.addDouble(r);
}
return true;
case VOCAB_HUE:
response.addDouble(getHue());
return true;
case VOCAB_SATURATION:
response.addDouble(getSaturation());
return true;
case VOCAB_GAMMA:
response.addDouble(getGamma());
return true;
case VOCAB_SHUTTER:
response.addDouble(getShutter());
return true;
case VOCAB_GAIN:
response.addDouble(getGain());
return true;
case VOCAB_IRIS:
response.addDouble(getIris());
return true;
/*
case VOCAB_CAPTURE_QUALITY:
response.addDouble(getCaptureQuality());
return true;
case VOCAB_OPTICAL_FILTER:
response.addDouble(getOpticalFilter());
return true;
*/
case VOCAB_WIDTH:
// normally, this would come from stream information
response.addInt(width());
return true;
case VOCAB_HEIGHT:
// normally, this would come from stream information
response.addInt(height());
return true;
}
return DeviceResponder::respond(cmd,response);
//////////////////
// DC1394 COMMANDS
//////////////////
default:
if (fgCtrlDC1394) switch(code)
{
case VOCAB_DRHASFEA: // VOCAB_DRHASFEA 00
response.addInt(int(fgCtrlDC1394->hasFeatureDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRSETVAL: // VOCAB_DRSETVAL 01
response.addInt(int(fgCtrlDC1394->setFeatureDC1394(cmd.get(1).asInt(),cmd.get(2).asDouble())));
return true;
case VOCAB_DRGETVAL: // VOCAB_DRGETVAL 02
response.addDouble(fgCtrlDC1394->getFeatureDC1394(cmd.get(1).asInt()));
return true;
case VOCAB_DRHASACT: // VOCAB_DRHASACT 03
response.addInt(int(fgCtrlDC1394->hasOnOffDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRSETACT: // VOCAB_DRSETACT 04
response.addInt(int(fgCtrlDC1394->setActiveDC1394(cmd.get(1).asInt(),(cmd.get(2).asInt()!=0))));
return true;
case VOCAB_DRGETACT: // VOCAB_DRGETACT 05
response.addInt(int(fgCtrlDC1394->getActiveDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRHASMAN: // VOCAB_DRHASMAN 06
response.addInt(int(fgCtrlDC1394->hasManualDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRHASAUT: // VOCAB_DRHASAUT 07
response.addInt(int(fgCtrlDC1394->hasAutoDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRHASONP: // VOCAB_DRHASONP 08
response.addInt(int(fgCtrlDC1394->hasOnePushDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRSETMOD: // VOCAB_DRSETMOD 09
response.addInt(int(fgCtrlDC1394->setModeDC1394(cmd.get(1).asInt(),(cmd.get(2).asInt()!=0))));
return true;
case VOCAB_DRGETMOD: // VOCAB_DRGETMOD 10
response.addInt(int(fgCtrlDC1394->getModeDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRSETONP: // VOCAB_DRSETONP 11
response.addInt(int(fgCtrlDC1394->setOnePushDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETMSK: // VOCAB_DRGETMSK 12
response.addInt(int(fgCtrlDC1394->getVideoModeMaskDC1394()));
return true;
case VOCAB_DRGETVMD: // VOCAB_DRGETVMD 13
response.addInt(int(fgCtrlDC1394->getVideoModeDC1394()));
return true;
case VOCAB_DRSETVMD: // VOCAB_DRSETVMD 14
response.addInt(int(fgCtrlDC1394->setVideoModeDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETFPM: // VOCAB_DRGETFPM 15
response.addInt(int(fgCtrlDC1394->getFPSMaskDC1394()));
return true;
case VOCAB_DRGETFPS: // VOCAB_DRGETFPS 16
response.addInt(int(fgCtrlDC1394->getFPSDC1394()));
return true;
case VOCAB_DRSETFPS: // VOCAB_DRSETFPS 17
response.addInt(int(fgCtrlDC1394->setFPSDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETISO: // VOCAB_DRGETISO 18
response.addInt(int(fgCtrlDC1394->getISOSpeedDC1394()));
return true;
case VOCAB_DRSETISO: // VOCAB_DRSETISO 19
response.addInt(int(fgCtrlDC1394->setISOSpeedDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETCCM: // VOCAB_DRGETCCM 20
response.addInt(int(fgCtrlDC1394->getColorCodingMaskDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETCOD: // VOCAB_DRGETCOD 21
response.addInt(int(fgCtrlDC1394->getColorCodingDC1394()));
return true;
case VOCAB_DRSETCOD: // VOCAB_DRSETCOD 22
response.addInt(int(fgCtrlDC1394->setColorCodingDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRSETWHB: // VOCAB_DRSETWHB 23
response.addInt(int(fgCtrlDC1394->setWhiteBalanceDC1394(cmd.get(1).asDouble(),cmd.get(2).asDouble())));
return true;
case VOCAB_DRGETWHB: // VOCAB_DRGETWHB 24
{
double b,r;
fgCtrlDC1394->getWhiteBalanceDC1394(b,r);
response.addDouble(b);
response.addDouble(r);
}
return true;
case VOCAB_DRGETF7M: // VOCAB_DRGETF7M 25
{
unsigned int xstep,ystep,xdim,ydim,xoffstep,yoffstep;
fgCtrlDC1394->getFormat7MaxWindowDC1394(xdim,ydim,xstep,ystep,xoffstep,yoffstep);
response.addInt(xdim);
response.addInt(ydim);
response.addInt(xstep);
response.addInt(ystep);
response.addInt(xoffstep);
response.addInt(yoffstep);
}
return true;
case VOCAB_DRGETWF7: // VOCAB_DRGETWF7 26
{
unsigned int xdim,ydim;
int x0,y0;
fgCtrlDC1394->getFormat7WindowDC1394(xdim,ydim,x0,y0);
response.addInt(xdim);
response.addInt(ydim);
response.addInt(x0);
response.addInt(y0);
}
return true;
case VOCAB_DRSETWF7: // VOCAB_DRSETWF7 27
response.addInt(int(fgCtrlDC1394->setFormat7WindowDC1394(cmd.get(1).asInt(),cmd.get(2).asInt(),cmd.get(3).asInt(),cmd.get(4).asInt())));
return true;
case VOCAB_DRSETOPM: // VOCAB_DRSETOPM 28
response.addInt(int(fgCtrlDC1394->setOperationModeDC1394(cmd.get(1).asInt()!=0)));
return true;
case VOCAB_DRGETOPM: // VOCAB_DRGETOPM 29
response.addInt(fgCtrlDC1394->getOperationModeDC1394());
return true;
case VOCAB_DRSETTXM: // VOCAB_DRSETTXM 30
response.addInt(int(fgCtrlDC1394->setTransmissionDC1394(cmd.get(1).asInt()!=0)));
return true;
case VOCAB_DRGETTXM: // VOCAB_DRGETTXM 31
response.addInt(fgCtrlDC1394->getTransmissionDC1394());
return true;
/*
case VOCAB_DRSETBAY: // VOCAB_DRSETBAY 32
response.addInt(int(fgCtrlDC1394->setBayerDC1394(bool(cmd.get(1).asInt()))));
return true;
case VOCAB_DRGETBAY: // VOCAB_DRGETBAY 33
response.addInt(fgCtrlDC1394->getBayerDC1394());
return true;
*/
case VOCAB_DRSETBCS: // VOCAB_DRSETBCS 34
response.addInt(int(fgCtrlDC1394->setBroadcastDC1394(cmd.get(1).asInt()!=0)));
return true;
case VOCAB_DRSETDEF: // VOCAB_DRSETDEF 35
response.addInt(int(fgCtrlDC1394->setDefaultsDC1394()));
return true;
case VOCAB_DRSETRST: // VOCAB_DRSETRST 36
response.addInt(int(fgCtrlDC1394->setResetDC1394()));
return true;
case VOCAB_DRSETPWR: // VOCAB_DRSETPWR 37
response.addInt(int(fgCtrlDC1394->setPowerDC1394(cmd.get(1).asInt()!=0)));
return true;
case VOCAB_DRSETCAP: // VOCAB_DRSETCAP 38
response.addInt(int(fgCtrlDC1394->setCaptureDC1394(cmd.get(1).asInt()!=0)));
return true;
case VOCAB_DRSETBPP: // VOCAB_DRSETCAP 39
response.addInt(int(fgCtrlDC1394->setBytesPerPacketDC1394(cmd.get(1).asInt())));
return true;
case VOCAB_DRGETBPP: // VOCAB_DRGETTXM 40
response.addInt(fgCtrlDC1394->getBytesPerPacketDC1394());
return true;
}
}
return DeviceResponder::respond(cmd,response);
}
void handleTorqueMsg(ITorqueControl *torque, const yarp::os::Bottle& cmd,
yarp::os::Bottle& response, bool *rec, bool *ok)
{
fprintf(stderr, "Handling ITorque messages\n");
if (!torque)
{
fprintf(stderr, "Error, I do not have a valid ITorque interface\n");
*ok=false;
return;
}
int controlledJoints;
torque->getAxes(&controlledJoints);
int code = cmd.get(1).asVocab();
switch (code)
{
case VOCAB_SET:
{
*rec = true;
switch(cmd.get(2).asVocab())
{
case VOCAB_REF:
{
*ok = torque->setRefTorque(cmd.get(3).asInt(), cmd.get(4).asDouble());
}
break;
case VOCAB_REFS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
double *p = new double[njs]; // LATER: optimize to avoid allocation.
for (i = 0; i < njs; i++)
p[i] = b.get(i).asDouble();
*ok = torque->setRefTorques (p);
delete[] p;
}
}
break;
case VOCAB_LIM:
{
*ok = torque->setTorqueErrorLimit (cmd.get(3).asInt(), cmd.get(4).asDouble());
}
break;
case VOCAB_LIMS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
double *p = new double[njs]; // LATER: optimize to avoid allocation.
for (i = 0; i < njs; i++)
p[i] = b.get(i).asDouble();
*ok = torque->setTorqueErrorLimits (p);
delete[] p;
}
}
break;
case VOCAB_PID:
{
Pid p;
int j = cmd.get(3).asInt();
Bottle& b = *(cmd.get(4).asList());
p.kp = b.get(0).asDouble();
p.kd = b.get(1).asDouble();
p.ki = b.get(2).asDouble();
p.max_int = b.get(3).asDouble();
p.max_output = b.get(4).asDouble();
p.offset = b.get(5).asDouble();
p.scale = b.get(6).asDouble();
*ok = torque->setTorquePid(j, p);
}
break;
case VOCAB_PIDS:
{
Bottle& b = *(cmd.get(3).asList());
int i;
const int njs = b.size();
if (njs==controlledJoints)
{
Pid *p = new Pid[njs];
for (i = 0; i < njs; i++)
{
Bottle& c = *(b.get(i).asList());
p[i].kp = c.get(0).asDouble();
p[i].kd = c.get(1).asDouble();
p[i].ki = c.get(2).asDouble();
p[i].max_int = c.get(3).asDouble();
p[i].max_output = c.get(4).asDouble();
p[i].offset = c.get(5).asDouble();
p[i].scale = c.get(6).asDouble();
}
*ok = torque->setTorquePids(p);
delete[] p;
}
}
break;
case VOCAB_RESET:
{
*ok = torque->resetTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_DISABLE:
{
*ok = torque->disableTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_ENABLE:
{
*ok = torque->enableTorquePid (cmd.get(3).asInt());
}
break;
case VOCAB_TORQUE_MODE:
{
*ok = torque->setTorqueMode();
}
break;
}
}
break;
case VOCAB_GET:
{
*rec = true;
int tmp = 0;
double dtmp = 0.0;
response.addVocab(VOCAB_IS);
response.add(cmd.get(1));
switch(cmd.get(2).asVocab())
{
case VOCAB_AXES:
{
int tmp;
*ok = torque->getAxes(&tmp);
response.addInt(tmp);
}
break;
case VOCAB_TRQ:
{
*ok = torque->getTorque(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_TRQS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorques(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_ERR:
{
*ok = torque->getTorqueError(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_ERRS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorqueErrors(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_OUTPUT:
{
*ok = torque->getTorquePidOutput(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_OUTPUTS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorquePidOutputs(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_PID:
{
Pid p;
*ok = torque->getTorquePid(cmd.get(3).asInt(), &p);
Bottle& b = response.addList();
b.addDouble(p.kp);
b.addDouble(p.kd);
b.addDouble(p.ki);
b.addDouble(p.max_int);
b.addDouble(p.max_output);
b.addDouble(p.offset);
b.addDouble(p.scale);
}
break;
case VOCAB_PIDS:
{
Pid *p = new Pid[controlledJoints];
*ok = torque->getTorquePids(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
{
Bottle& c = b.addList();
c.addDouble(p[i].kp);
c.addDouble(p[i].kd);
c.addDouble(p[i].ki);
c.addDouble(p[i].max_int);
c.addDouble(p[i].max_output);
c.addDouble(p[i].offset);
c.addDouble(p[i].scale);
}
delete[] p;
}
break;
case VOCAB_REFERENCE:
{
*ok = torque->getRefTorque(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_REFERENCES:
{
double *p = new double[controlledJoints];
*ok = torque->getRefTorques(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
case VOCAB_LIM:
{
*ok = torque->getTorqueErrorLimit(cmd.get(3).asInt(), &dtmp);
response.addDouble(dtmp);
}
break;
case VOCAB_LIMS:
{
double *p = new double[controlledJoints];
*ok = torque->getTorqueErrorLimits(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(p[i]);
delete[] p;
}
break;
}
}
break;
}
//rec --> true se il comando e' riconosciuto
//ok --> contiene il return value della chiamata all'interfaccia
// ...*ok=torque->setPid();
//torque->
}
// Callback handler for RPC commands (?)
bool RpcMsgHandler::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply)
{
bool ok = true;
bool commandUnderstood = true; // is the command recognized?
if (caller->verbose())
printf("command received: %s\n", cmd.toString().c_str());
int code = cmd.get(0).asVocab();
switch (code)
{
case VOCAB_GET:
{
double dtmp = 0.0;
double dtmp2 = 0.0;
reply.addVocab(VOCAB_IS);
reply.add(cmd.get(1));
switch(cmd.get(1).asVocab())
{
case VOCAB_AXES:
{
int axes;
caller->getAxes(axes);
reply.addInt(axes);
}
break;
case VOCAB_DEBUG_PARAMETER:
{
int j = cmd.get(2).asInt();
int index = cmd.get(3).asInt();
ok = caller->getDebugParameter(j, index, &dtmp);
reply.addInt(j);
reply.addInt(index);
reply.addDouble(dtmp);
}
break;
case VOCAB_GENERIC_PARAMETER:
{
int j = cmd.get(2).asInt();
int param = cmd.get(3).asInt();
ok = caller->getParameter(j, param, &dtmp);
reply.addInt(j);
reply.addInt(param);
reply.addDouble(dtmp);
}
break;
default:
{
commandUnderstood = false;
std::cout << "Debug Interface 1: command not understood! received " << cmd.toString().c_str() << std::endl;
}
break;
}
}
break; // case VOCAB_GET
case VOCAB_SET:
{
switch(cmd.get(1).asVocab())
{
case VOCAB_GENERIC_PARAMETER:
{
int j = cmd.get(2).asInt();
int param = cmd.get(3).asInt();
double val = cmd.get(4).asDouble();
ok = caller->setParameter(j, param, val);
}
break;
case VOCAB_DEBUG_PARAMETER:
{
int j = cmd.get(2).asInt();
int index = cmd.get(3).asInt();
double val = cmd.get(4).asDouble();
ok = caller->setDebugParameter(j, index, val);
}
break;
default:
{
commandUnderstood = false;
std::cout << "Debug Interface 2: command not understood! received " << cmd.toString().c_str() << std::endl;
}
break;
}
}
break; // case VOCAB_SET
default:
{
commandUnderstood = false;
std::cout << "Debug Interface 3: command not understood! received " << cmd.toString().c_str() << std::endl;
}
break;
} //switch code
if (!commandUnderstood)
{
ok = DeviceResponder::respond(cmd,reply);
}
if (!ok)
{
// failed thus send only a VOCAB back.
reply.clear();
reply.addVocab(VOCAB_FAILED);
}
else
reply.addVocab(VOCAB_OK);
return ok;
}
virtual bool respond(const yarp::os::Bottle& command,
yarp::os::Bottle& reply) {
mutex.wait();
switch (command.get(0).asVocab()) {
case VOCAB3('a','d','d'):
{
string name = command.get(1).asString().c_str();
if (name!="") {
removeName(name.c_str());
Entry entry;
entry.name = name;
q.push_back(entry);
reply.clear();
reply.add(Value::makeVocab("add"));
reply.addString(name.c_str());
addQueue(reply);
}
}
break;
case VOCAB3('d','e','l'):
{
if (command.get(1).isInt()) {
int idx = command.get(1).asInt();
bool acted = removeName(idx);
if (acted) {
reply.clear();
reply.add(Value::makeVocab("del"));
reply.addInt(idx);
} else {
reply.clear();
reply.add(Value::makeVocab("no"));
reply.addInt(idx);
}
addQueue(reply);
} else {
string name = command.get(1).asString().c_str();
if (name!="") {
bool acted = removeName(name.c_str());
if (acted) {
reply.clear();
reply.add(Value::makeVocab("del"));
reply.addString(name.c_str());
} else {
reply.clear();
reply.add(Value::makeVocab("no"));
reply.addString(name.c_str());
}
addQueue(reply);
}
}
}
break;
case VOCAB4('l','i','s','t'):
{
reply.clear();
addQueue(reply);
}
break;
default:
updateHelp();
mutex.post();
return DeviceResponder::respond(command,reply);
}
mutex.post();
printf("%s\n", reply.toString().c_str());
return true;
}
bool DispatcherManager::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply) {
bool success = false;
try {
switch(cmd.get(0).asVocab()) {
case VOCAB4('h','e','l','p'): // print help information
reply.add(yarp::os::Value::makeVocab("help"));
reply.addString("Event Manager configuration options");
reply.addString(" help Displays this message");
reply.addString(" list Print a list of available event listeners");
reply.addString(" add type [type2 ...] Adds one or more event listeners");
reply.addString(" remove [all|idx] Removes event listener at an index or all");
reply.addString(" set [all|idx] Configures a listener");
reply.addString(" stats Prints information");
success = true;
break;
case VOCAB4('l','i','s','t'): // print list of available event listeners
{
reply.add(yarp::os::Value::makeVocab("help"));
std::vector<std::string> keys = FactoryT<std::string, IEventListener>::instance().getKeys();
for(unsigned int i = 0; i < keys.size(); i++) {
reply.addString((std::string(" ") + keys[i]).c_str());
}
success = true;
break;
}
case VOCAB3('a','d','d'): // add
{ // prevent identifier initialization to cross borders of case
yarp::os::Bottle list = cmd.tail();
for(int i = 0; i < list.size(); i++) {
IEventListener* listener = this->factory->create(list.get(i).asString().c_str());
listener->start();
this->dispatcher->addListener(listener);
}
reply.addString("Successfully added listener(s)");
success = true;
break;
}
case VOCAB4('r','e','m','o'): // remove
case VOCAB3('d','e','l'): // del(ete)
{ // prevent identifier initialization to cross borders of case
if(cmd.get(1).isInt() && cmd.get(1).asInt() >= 1 && cmd.get(1).asInt() <= this->dispatcher->countListeners()) {
this->dispatcher->removeListener(cmd.get(1).asInt()-1);
reply.addString("Successfully removed listener.");
success = true;
} else if(cmd.get(1).asString() == "all") {
this->dispatcher->clear();
reply.addString("Successfully removed all listeners.");
success = true;
} else {
throw std::runtime_error("Illegal index!");
}
break;
}
case VOCAB3('s','e','t'): // set
{ // prevent identifier initialization to cross borders of case
yarp::os::Bottle property;
property.addList() = cmd.tail().tail(); // see comment in TrainModule
std::string replymsg = "Setting configuration option ";
if(cmd.get(1).isInt() && cmd.get(1).asInt() >= 1 &&
cmd.get(1).asInt() <= this->dispatcher->countListeners()) {
bool ok = this->dispatcher->getAt(cmd.get(1).asInt()-1).configure(property);
replymsg += ok ? "succeeded" :
"failed; please check key and value type.";
reply.addString(replymsg.c_str());
success = true;
} else if(cmd.get(1).asString() == "all") {
for(int i = 0; i < this->dispatcher->countListeners(); i++) {
if(i > 0) {
replymsg += ", ";
}
bool ok = this->dispatcher->getAt(i).configure(property);
replymsg += ok ? "succeeded" :
"failed; please check key and value type.";
}
replymsg += ".";
reply.addString(replymsg.c_str());
success = true;
} else {
throw std::runtime_error("Illegal index!");
}
break;
}
case VOCAB4('i','n','f','o'): // information
case VOCAB4('s','t','a','t'): // statistics
{ // prevent identifier initialization to cross borders of case
reply.add(yarp::os::Value::makeVocab("help"));
std::ostringstream buffer;
buffer << "Event Manager Information (" << this->dispatcher->countListeners() << " listeners)";
reply.addString(buffer.str().c_str());
for(int i = 0; i < this->dispatcher->countListeners(); i++) {
buffer.str(""); // why isn't there a proper reset method?
buffer << " [" << (i + 1) << "] " << this->dispatcher->getAt(i).getInfo();
reply.addString(buffer.str().c_str());
}
success = true;
break;
}
default:
break;
}
} catch(const std::exception& e) {
std::string msg = std::string("Error: ") + e.what();
reply.addString(msg.c_str());
success = true;
}
return success;
}
bool TrainModule::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply) {
// NOTE: the module class spawns a new thread, which implies that exception
// handling needs to be done in this thread, so not the 'main' thread.
bool success = false;
try {
switch(cmd.get(0).asVocab()) {
case VOCAB4('h','e','l','p'): // print help information
reply.add(yarp::os::Value::makeVocab("help"));
reply.addString("Training module configuration options");
reply.addString(" help Displays this message");
reply.addString(" train Trains the machine and sends the model");
reply.addString(" model Sends the model to the prediction module");
reply.addString(" reset Resets the machine to its current state");
reply.addString(" info Outputs information about the machine");
reply.addString(" pause Disable passing the samples to the machine");
reply.addString(" continue Enable passing the samples to the machine");
reply.addString(" set key val Sets a configuration option for the machine");
reply.addString(" load fname Loads a machine from a file");
reply.addString(" save fname Saves the current machine to a file");
reply.addString(" event [cmd ...] Sends commands to event dispatcher (see: event help)");
reply.addString(" cmd fname Loads commands from a file");
reply.addString(this->getMachine().getConfigHelp().c_str());
success = true;
break;
case VOCAB4('t','r','a','i'): // train the machine, implies sending model
this->getMachine().train();
reply.addString("Training completed.");
case VOCAB4('m','o','d','e'): // send model
this->model_out.write(this->machinePortable);
reply.addString("The model has been written to the port.");
success = true;
break;
case VOCAB4('c','l','e','a'): // clear machine
case VOCAB3('c','l','r'):
case VOCAB4('r','e','s','e'): // reset
case VOCAB3('r','s','t'):
this->getMachine().reset();
reply.addString("Machine cleared.");
success = true;
break;
case VOCAB4('p','a','u','s'): // pause sample stream
case VOCAB4('d','i','s','a'): // disable
this->trainProcessor.setEnabled(false);
reply.addString("Sample stream to machine disabled.");
success = true;
break;
case VOCAB4('c','o','n','t'): // continue sample stream
case VOCAB4('e','n','a','b'): // enable
this->trainProcessor.setEnabled(true);
reply.addString("Sample stream to machine enabled.");
success = true;
break;
case VOCAB4('i','n','f','o'): // information
case VOCAB4('s','t','a','t'): // statistics
{ // prevent identifier initialization to cross borders of case
reply.add(yarp::os::Value::makeVocab("help"));
reply.addString("Machine Information: ");
reply.addString(this->getMachine().getInfo().c_str());
success = true;
break;
}
case VOCAB4('l','o','a','d'): // load
{ // prevent identifier initialization to cross borders of case
reply.add(yarp::os::Value::makeVocab("help"));
std::string replymsg = std::string("Loading machine from '") +
cmd.get(1).asString().c_str() + "'... " ;
if(!cmd.get(1).isString()) {
replymsg += "failed";
} else {
this->getMachinePortable().readFromFile(cmd.get(1).asString().c_str());
replymsg += "succeeded";
}
reply.addString(replymsg.c_str());
success = true;
break;
}
case VOCAB4('s','a','v','e'): // save
{ // prevent identifier initialization to cross borders of case
reply.add(yarp::os::Value::makeVocab("help"));
std::string replymsg = std::string("Saving machine to '") +
cmd.get(1).asString().c_str() + "'... " ;
if(!cmd.get(1).isString()) {
replymsg += "failed";
} else {
this->getMachinePortable().writeToFile(cmd.get(1).asString().c_str());
replymsg += "succeeded";
}
reply.addString(replymsg.c_str());
success = true;
break;
}
case VOCAB3('s','e','t'): // set a configuration option for the machine
{ // prevent identifier initialization to cross borders of case
yarp::os::Bottle property;
/*
* This is a simple hack to enable multiple parameters The need
* for this hack lies in the fact that a group can only be found
* using findGroup if it is a nested list in a Bottle. If the
* Bottle itself is the list, then the group will _not_ be found.
*/
property.addList() = cmd.tail();
std::string replymsg = "Setting configuration option ";
bool ok = this->getMachine().configure(property);
replymsg += ok ? "succeeded" :
"failed; please check key and value type.";
reply.addString(replymsg.c_str());
success = true;
break;
}
case VOCAB4('e','v','e','n'): // event
{ // prevent identifier initialization to cross borders of case
success = this->dmanager.respond(cmd.tail(), reply);
break;
}
case VOCAB3('c','m','d'): // cmd
case VOCAB4('c','o','m','m'): // command
{ // prevent identifier initialization to cross borders of case
reply.add(yarp::os::Value::makeVocab("help"));
std::string replymsg;
if(!cmd.get(1).isString()) {
replymsg = "Please supply a valid filename.";
} else {
std::string full_fname = this->findFile(cmd.get(1).asString().c_str());
replymsg = std::string("Loading commands from '") +
full_fname + "'... " ;
this->loadCommandFile(full_fname, &reply);
replymsg += "succeeded";
}
reply.addString(replymsg.c_str());
success = true;
break;
}
default:
break;
}
} catch(const std::exception& e) {
std::string msg = std::string("Error: ") + e.what();
reply.addString(msg.c_str());
success = true;
}
return success;
}