/* Respond function */
bool babbler::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
std::string helpMessage = std::string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"F + (int)Frequency\n" +
"RF \n" +
"quit \n";
bReply.clear();
std::string keyWord = bCommand.get(0).asString().c_str();
if (keyWord == "quit") {
bReply.addString("quitting");
return false;
}
else if (keyWord == "help") {
cout << helpMessage;
bReply.addString("ok");
}
else if (keyWord == "F") {
if (bCommand.size() == 2)
{
if (bCommand.get(1).isInt())
{
setNewFrequency(bCommand.get(1).asInt());
bReply.addInt(f);
}
}
}
else if (keyWord == "RF") {
bReply.addInt(newRandomFrequency());
}
return true;
}
bool Implement_DepthVisualParams_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
bool ret = false;
response.clear();
if(!iDepthVisual)
{
yError() << "Depth Visual parameter Parser has not been correctly configured. IDepthVisualParams interface is not valid";
response.addVocab(VOCAB_FAILED);
return false;
}
int code = cmd.get(0).asVocab();
if(code != VOCAB_DEPTH_VISUAL_PARAMS)
{
yError() << "Depth Visual Params Parser received a command not belonging to this interface. Required interface was " << yarp::os::Vocab::decode(code);
response.addVocab(VOCAB_FAILED);
return false;
}
switch (cmd.get(1).asVocab())
{
case VOCAB_GET:
{
switch(cmd.get(2).asVocab())
{
case VOCAB_HEIGHT:
{
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_HEIGHT);
response.addVocab(VOCAB_IS);
response.addInt(iDepthVisual->getDepthHeight());
}
break;
case VOCAB_WIDTH:
{
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_WIDTH);
response.addVocab(VOCAB_IS);
response.addInt(iDepthVisual->getDepthWidth());
}
break;
case VOCAB_FOV:
{
double hFov, vFov;
ret = iDepthVisual->getDepthFOV(hFov, vFov);
if(ret)
{
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_FOV);
response.addVocab(VOCAB_IS);
response.addDouble(hFov);
response.addDouble(vFov);
}
else
response.addVocab(VOCAB_FAILED);
}
break;
case VOCAB_INTRINSIC_PARAM:
{
yarp::os::Property params;
ret = iDepthVisual->getDepthIntrinsicParam(params);
if(ret)
{
yarp::os::Bottle params_b;
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_INTRINSIC_PARAM);
response.addVocab(VOCAB_IS);
Property::copyPortable(params, params_b); // will it really work??
response.append(params_b);
}
else
{
response.addVocab(VOCAB_FAILED);
}
}
break;
case VOCAB_ACCURACY:
{
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_ACCURACY);
response.addVocab(VOCAB_IS);
response.addDouble(iDepthVisual->getDepthAccuracy());
}
break;
case VOCAB_CLIP_PLANES:
{
double nearPlane, farPlane;
iDepthVisual->getDepthClipPlanes(nearPlane, farPlane);
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_CLIP_PLANES);
response.addVocab(VOCAB_IS);
response.addDouble(nearPlane);
response.addDouble(farPlane);
}
break;
case VOCAB_MIRROR:
{
bool mirror;
ret = iDepthVisual->getDepthMirroring(mirror);
if(ret)
{
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_MIRROR);
response.addVocab(VOCAB_IS);
response.addInt(mirror);
}
else
response.addVocab(VOCAB_FAILED);
}
break;
default:
{
yError() << "Depth Visual Parameter interface parser received am unknown GET command. Command is " << cmd.toString();
response.addVocab(VOCAB_FAILED);
ret = false;
}
break;
}
}
break;
case VOCAB_SET:
{
switch(cmd.get(2).asVocab())
{
case VOCAB_RESOLUTION:
{
ret = iDepthVisual->setDepthResolution(cmd.get(3).asInt(), cmd.get(4).asInt());
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_SET);
response.addInt(ret);
}
break;
case VOCAB_FOV:
{
ret = iDepthVisual->setDepthFOV(cmd.get(3).asDouble(), cmd.get(4).asDouble());
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_SET);
response.addInt(ret);
}
break;
case VOCAB_ACCURACY:
{
ret = iDepthVisual->setDepthAccuracy(cmd.get(3).asDouble());
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_SET);
response.addInt(ret);
}
break;
case VOCAB_CLIP_PLANES:
{
ret = iDepthVisual->setDepthClipPlanes(cmd.get(3).asDouble(), cmd.get(4).asDouble());
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_SET);
response.addInt(ret);
}
break;
case VOCAB_INTRINSIC_PARAM:
{
yarp::os::Property params;
ret = iDepthVisual->getDepthIntrinsicParam(params);
if(ret)
{
yarp::os::Bottle params_b;
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_INTRINSIC_PARAM);
response.addVocab(VOCAB_IS);
Property::copyPortable(params, params_b); // will it really work??
response.append(params_b);
}
else
response.addVocab(VOCAB_FAILED);
}
break;
case VOCAB_MIRROR:
{
ret = iDepthVisual->setDepthMirroring(cmd.get(3).asBool());
response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
response.addVocab(VOCAB_SET);
response.addInt(ret);
}
break;
default:
{
yError() << "Rgb Visual Parameter interface parser received am unknown SET command. Command is " << cmd.toString();
response.addVocab(VOCAB_FAILED);
ret = false;
}
break;
}
}
break;
default:
{
yError() << "Rgb Visual parameter interface Parser received a malformed request. Command should either be 'set' or 'get', received " << cmd.toString();
response.addVocab(VOCAB_FAILED);
ret = false;
}
break;
}
return ret;
}
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 handleInteractionModeMsg(IInteractionMode *iInteract, const yarp::os::Bottle& cmd,
yarp::os::Bottle& response, bool *rec, bool *ok)
{
fprintf(stderr, "Handling IInteractionMode message %s\n", cmd.toString().c_str());
if (!iInteract)
{
fprintf(stderr, "Error I do not have a valid interface\n");
*ok=false;
return;
}
int code = cmd.get(1).asVocab();
*ok=true;
switch(code)
{
case VOCAB_SET:
{
int axis = cmd.get(3).asInt();
yarp::dev::InteractionModeEnum mode = (yarp::dev::InteractionModeEnum) cmd.get(2).asVocab();
*ok = iInteract->setInteractionMode(axis, mode);
*rec=true; //or false
}
break;
case VOCAB_GET:
{
int which = cmd.get(2).asVocab();
switch(which)
{
case VOCAB_INTERACTION_MODE:
{
int axis = cmd.get(3).asInt();
yarp::dev::InteractionModeEnum mode;
*ok = iInteract->getInteractionMode(axis, &mode);
// create response
if(*ok)
{
response.addVocab(VOCAB_IS);
response.addInt(axis);
response.addVocab(VOCAB_INTERACTION_MODE);
response.addVocab(mode);
*rec=true;
}
else
{
response.addVocab(VOCAB_FAILED);
*rec = false;
}
}
break;
case VOCAB_INTERACTION_MODES:
{
int axis = cmd.get(3).asInt();
yarp::dev::InteractionModeEnum *modes;
modes = new yarp::dev::InteractionModeEnum[jnts];
*ok = iInteract->getInteractionMode(axis, modes);
// create response
if(*ok)
{
response.addVocab(VOCAB_IS);
response.addVocab(VOCAB_INTERACTION_MODES);
for(int i=0; i<jnts; i++)
response.addVocab(modes[i]);
*rec=true;
}
else
{
response.addVocab(VOCAB_FAILED);
*rec = false;
}
}
break;
default:
{
fprintf(stderr, "get command not understood");
*rec=false;
break;
}
break;
}
}
break;
default:
{
fprintf(stderr, "type of command not understood");
*rec=false;
}
break;
}
}
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->
}
/*! @brief Fill a bottle with the contents of an object.
@param[in] jct The %JavaScript engine context.
@param[in,out] aBottle The bottle to be filled.
@param[in] theData The value to be sent.
@param[in] topLevel @c true if this is the outermost list of an object. */
static void
fillBottleFromValue(JSContext * jct,
yarp::os::Bottle & aBottle,
JS::Value theData,
const bool topLevel)
{
ODL_ENTER(); //####
ODL_P2("jct = ", jct, "aBottle = ", &aBottle); //####
ODL_B1("topLevel = ", topLevel); //####
JS::RootedValue asRootedValue(jct);
asRootedValue = theData;
if (theData.isBoolean())
{
aBottle.addInt(JS::ToBoolean(asRootedValue) ? 1 : 0);
}
else if (theData.isDouble())
{
double aValue;
if (JS::ToNumber(jct, asRootedValue, &aValue))
{
aBottle.addDouble(aValue);
}
}
else if (theData.isInt32())
{
int32_t aValue;
if (JS::ToInt32(jct, asRootedValue, &aValue))
{
aBottle.addInt(aValue);
}
}
else if (theData.isString())
{
JSString * asString = theData.toString();
char * asChars = JS_EncodeString(jct, asString);
aBottle.addString(asChars);
JS_free(jct, asChars);
}
else if (theData.isObject())
{
JS::RootedObject asObject(jct);
if (JS_ValueToObject(jct, asRootedValue, &asObject))
{
bool processed = false;
#if (47 <= MOZJS_MAJOR_VERSION)
bool isArray;
#endif // 47 <= MOZJS_MAJOR_VERSION
#if (47 <= MOZJS_MAJOR_VERSION)
if (JS_IsArrayObject(jct, asObject, &isArray))
#else // 47 > MOZJS_MAJOR_VERSION
if (JS_IsArrayObject(jct, asObject))
#endif // 47 > MOZJS_MAJOR_VERSION
{
uint32_t arrayLength;
if (JS_GetArrayLength(jct, asObject, &arrayLength))
{
// Treat as a list
if (topLevel)
{
for (uint32_t ii = 0; arrayLength > ii; ++ii)
{
JS::RootedValue anElement(jct);
if (JS_GetElement(jct, asObject, ii, &anElement))
{
fillBottleFromValue(jct, aBottle, anElement, false);
}
}
}
else
{
yarp::os::Bottle & innerList(aBottle.addList());
for (uint32_t ii = 0; arrayLength > ii; ++ii)
{
JS::RootedValue anElement(jct);
if (JS_GetElement(jct, asObject, ii, &anElement))
{
fillBottleFromValue(jct, innerList, anElement, false);
}
}
}
processed = true;
}
}
if (! processed)
{
// Treat as a dictionary
yarp::os::Property & innerDict(aBottle.addDict());
#if (47 <= MOZJS_MAJOR_VERSION)
JS::Rooted<JS::IdVector> ids(jct, JS::IdVector(jct));
#else // 47 > MOZJS_MAJOR_VERSION
JS::AutoIdArray ids(jct, JS_Enumerate(jct, asObject));
#endif // 47 > MOZJS_MAJOR_VERSION
#if (47 <= MOZJS_MAJOR_VERSION)
if (JS_Enumerate(jct, asObject, &ids))
#else // 47 > MOZJS_MAJOR_VERSION
// Note that only operator! is defined, so we need to do a 'double-negative'.
if (!! ids)
#endif // 47 > MOZJS_MAJOR_VERSION
{
for (size_t ii = 0, len = ids.length(); len > ii; ++ii)
{
JS::RootedValue key(jct);
if (JS_IdToValue(jct, ids[ii], &key))
{
JS::RootedValue key(jct);
JS::RootedValue result(jct);
JS::RootedId aRootedId(jct);
aRootedId = ids[ii];
if (JS_IdToValue(jct, ids[ii], &key) &&
JS_GetPropertyById(jct, asObject, aRootedId, &result))
{
JSString * keyString = key.toString();
char * keyAsChars = JS_EncodeString(jct, keyString);
YarpString keyToUse(keyAsChars);
yarp::os::Bottle convertedResult;
JS_free(jct, keyAsChars);
fillBottleFromValue(jct, convertedResult, result, false);
if (1 == convertedResult.size())
{
yarp::os::Value anElement(convertedResult.get(0));
if (anElement.isInt())
{
innerDict.put(keyToUse, anElement.asInt());
}
else if (anElement.isDouble())
{
innerDict.put(keyToUse, anElement.asDouble());
}
else if (anElement.isString())
{
innerDict.put(keyToUse, anElement.asString());
}
else
{
innerDict.put(keyToUse, anElement);
}
}
}
}
}
}
}
}
}
ODL_EXIT(); //####
} // fillBottleFromValue
void handleControlModeMsg(IControlMode2 *iMode, const yarp::os::Bottle& cmd,
yarp::os::Bottle& response, bool *rec, bool *ok)
{
fprintf(stderr, "Handling IControlMode message %s\n", cmd.toString().c_str());
if (!iMode)
{
fprintf(stderr, "Error I do not have a valid interface\n");
*ok=false;
return;
}
int code = cmd.get(1).asVocab();
*ok=true;
switch(code)
{
case VOCAB_SET:
{
int axis = cmd.get(3).asInt();
int mode=cmd.get(2).asVocab();
switch (mode)
{
case VOCAB_CM_POSITION:
*ok = iMode->setPositionMode(axis);
break;
case VOCAB_CM_POSITION_DIRECT:
*ok = iMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT);
break;
case VOCAB_CM_VELOCITY:
*ok = iMode->setVelocityMode(axis);
break;
case VOCAB_CM_MIXED:
*ok = iMode->setControlMode(axis, VOCAB_CM_MIXED);
break;
case VOCAB_CM_TORQUE:
*ok = iMode->setTorqueMode(axis);
break;
case VOCAB_CM_OPENLOOP:
*ok = iMode->setOpenLoopMode(axis);
break;
case VOCAB_CM_IDLE:
*ok = iMode->setControlMode(axis, VOCAB_CM_IDLE);
break;
case VOCAB_CM_FORCE_IDLE:
*ok = iMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE);
break;
case VOCAB_CM_IMPEDANCE_POS:
*ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_POS);
break;
case VOCAB_CM_IMPEDANCE_VEL:
*ok = iMode->setControlMode(axis, VOCAB_CM_IMPEDANCE_VEL);
break;
default:
*ok = false;
break;
}
*rec=true; //or false
}
break;
case VOCAB_GET:
{
if (cmd.get(2).asVocab()==VOCAB_CM_CONTROL_MODE)
{
int p=-1;
int axis = cmd.get(3).asInt();
fprintf(stderr, "Calling getControlMode\n");
*ok = iMode->getControlMode(axis, &p);
response.addVocab(VOCAB_IS);
response.addInt(axis);
response.addVocab(VOCAB_CM_CONTROL_MODE);
response.addVocab(p);
//fprintf(stderr, "Returning %d\n", p);
*rec=true;
}
}
break;
default:
{
*rec=false;
}
break;
}
}
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;
}
// 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;
}
void TaskYarpInterface::parseIncomingMessage(yarp::os::Bottle& input, yarp::os::Bottle& reply)
{
int btlSize = input.size();
for (auto i=0; i<btlSize; ++i) {
int msg;
int testInt = input.get(0).asInt();
std::string testStr = input.get(0).asString();
if ((testStr=="") && (testInt!=0)) {
msg = TASK_MESSAGE(testInt);
} else {
msg = TaskMessageHandler::stringToTaskManagerMessageTag(testStr);
}
switch (msg) {
case GET_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_STATE";
}
TaskState state = this->task->getTaskState();
state.putIntoBottle(reply);
}break;
case GET_STIFFNESS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_STIFFNESS";
}
util::pourEigenMatrixIntoBottle(this->getStiffnessMatrix(), reply);
}break;
case GET_DAMPING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DAMPING";
}
util::pourEigenMatrixIntoBottle(this->getDampingMatrix(), reply);
}break;
case GET_WEIGHTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_WEIGHTS";
}
util::pourEigenVectorIntoBottle(this->getWeight(), reply);
}break;
case GET_DESIRED_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DESIRED_TASK_STATE";
}
TaskState state = this->task->getDesiredTaskState();
state.putIntoBottle(reply);
}break;
case GET_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_POSITION";
}
if ((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) {
Eigen::Vector3d pos = this->task->getTaskState().getPosition().getTranslation();
util::pourEigenVectorIntoBottle(pos, reply);
}
}break;
case GET_DESIRED_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DESIRED_TASK_POSITION";
}
if ((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) {
Eigen::Vector3d pos = this->task->getDesiredTaskState().getPosition().getTranslation();
util::pourEigenVectorIntoBottle(pos, reply);
}
}break;
case GET_ACTIVITY_STATUS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_ACTIVITY_STATUS";
}
if (this->isActivated()) {
reply.addInt(TASK_IS_ACTIVATED);
} else {
reply.addInt(TASK_IS_DEACTIVATED);
}
}break;
case GET_DIMENSION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DIMENSION";
}
OCRA_WARNING("this->task->getWeight() " << this->task->getWeight().size())
reply.addInt(this->task->getWeight().size());
}break;
case GET_TYPE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TYPE";
}
reply.addInt(this->task->getMetaTaskType());
}break;
case GET_TYPE_AS_STRING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TYPE";
}
reply.addString(this->task->getMetaTaskTypeAsString());
}break;
case GET_NAME:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_NAME";
}
reply.addString(this->task->getName());
}break;
case GET_CONTROL_PORT_NAMES:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_CONTROL_PORT_NAMES";
}
reply.addString(this->inputControlPortName);
reply.addString(this->outputControlPortName);
reply.addString(this->desiredStateOutputPortName);
}break;
case GET_TASK_PORT_NAME:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_PORT_NAME";
}
reply.addString(this->getPortName());
}break;
case GET_TASK_ERROR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_ERROR";
}
util::pourEigenVectorIntoBottle(this->getTaskError(), reply);
}break;
case SET_STIFFNESS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS";
}
++i;
setStiffness(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_STIFFNESS_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS_VECTOR";
}
int indexesToSkip;
this->setStiffness(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_STIFFNESS_MATRIX:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS_MATRIX";
}
int indexesToSkip;
this->setStiffness(util::pourBottleIntoEigenMatrix(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING";
}
++i;
this->setDamping(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING_VECTOR";
}
int indexesToSkip;
this->setDamping(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING_MATRIX:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING_MATRIX";
}
int indexesToSkip;
this->setDamping(util::pourBottleIntoEigenMatrix(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_WEIGHT:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_WEIGHT";
}
++i;
this->setWeight(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_WEIGHT_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_WEIGHT_VECTOR";
}
int indexesToSkip;
this->setWeight(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DESIRED_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DESIRED_TASK_STATE";
}
int indexesToSkip;
TaskState state;
bool extractSuccess = state.extractFromBottle(util::trimBottle(input, i+1), indexesToSkip);
i += indexesToSkip;
if (extractSuccess) {
// TODO: Set to setDesiredTaskState (needs internal traj gen.) See Task::setDesiredTaskState()
this->setDesiredTaskStateDirect(state);
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case SET_DESIRED_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DESIRED_TASK_POSITION";
}
if (((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) && (input.size()>=4)){
Eigen::Vector3d pos(input.get(i+1).asDouble(), input.get(i+2).asDouble(), input.get(i+3).asDouble());
TaskState state;
state.setPosition(Eigen::Displacementd(pos, Eigen::Rotation3d::Identity()));
// TODO: Set to setDesiredTaskState (needs internal traj gen.) See Task::setDesiredTaskState()
this->setDesiredTaskStateDirect(state);
i += 3;
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case ACTIVATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: ACTIVATE";
}
if(this->activate()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case DEACTIVATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: DEACTIVATE";
}
if(this->deactivate()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case OPEN_CONTROL_PORTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: OPEN_CONTROL_PORTS";
}
if(openControlPorts()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case CLOSE_CONTROL_PORTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: CLOSE_CONTROL_PORTS";
}
if(closeControlPorts()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case HELP:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: HELP";
}
}break;
default:
{
yLog.warning() << "Unrecognized request tag.";
}break;
}
}
}
bool FrameGrabberControls2_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
bool ok = false;
int action = cmd.get(1).asVocab();
int param = cmd.get(2).asVocab();
// yTrace() << "cmd received\n\t" << cmd.toString().c_str();
if(!fgCtrl2)
{
yError() << " Selected camera device has no IFrameGrabberControl2 interface";
return false;
}
response.clear();
switch (action)
{
case VOCAB_HAS:
{
response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
response.addVocab(VOCAB_HAS);
response.addVocab(VOCAB_FEATURE);
response.addInt(param);
switch (param)
{
case VOCAB_FEATURE:
{
bool _hasFeat;
ok = fgCtrl2->hasFeature(cmd.get(3).asInt(), &_hasFeat);
response.addInt(_hasFeat);
} break;
case VOCAB_ONOFF:
{
bool _hasOnOff;
ok = fgCtrl2->hasOnOff(cmd.get(3).asInt(), &_hasOnOff);
response.addInt(_hasOnOff);
} break;
case VOCAB_AUTO:
{
bool _hasAuto;
ok = fgCtrl2->hasAuto(cmd.get(3).asInt(), &_hasAuto);
response.addInt(_hasAuto);
} break;
case VOCAB_MANUAL:
{
bool _hasManual;
ok = fgCtrl2->hasManual(cmd.get(3).asInt(), &_hasManual);
response.addInt(_hasManual);
} break;
case VOCAB_ONEPUSH:
{
bool _hasOnePush;
ok = fgCtrl2->hasOnePush(cmd.get(3).asInt(), &_hasOnePush);
response.addInt(_hasOnePush);
} break;
default:
{
yError() << "Unknown command 'HAS " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
response.clear();
ok = false;
} break;
} break; // end switch (param)
} break; // end VOCAB_HAS
case VOCAB_SET:
{
switch (param)
{
case VOCAB_FEATURE:
{
ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble());
} break;
case VOCAB_FEATURE2:
{
ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble(), cmd.get(5).asDouble());
} break;
case VOCAB_ACTIVE:
{
ok = fgCtrl2->setActive(cmd.get(3).asInt(), cmd.get(4).asInt());
} break;
case VOCAB_MODE:
{
ok = fgCtrl2->setMode(cmd.get(3).asInt(), (FeatureMode) cmd.get(4).asInt());
} break;
case VOCAB_ONEPUSH:
{
ok = fgCtrl2->setOnePush(cmd.get(3).asInt());
} break;
default:
{
yError() << "Unknown command 'SET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
response.clear();
ok = false;
}
} break; // end switch (param)
} break; // end VOCAB_SET
case VOCAB_GET:
{
response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
response.addVocab(param);
response.addVocab(VOCAB_IS);
switch (param)
{
case VOCAB_CAMERA_DESCRIPTION:
{
CameraDescriptor camera;
ok = fgCtrl2->getCameraDescription(&camera);
response.addInt(camera.busType);
response.addString(camera.deviceDescription);
yDebug() << "Response is " << response.toString();
} break;
case VOCAB_FEATURE:
{
double value;
ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value);
response.addDouble(value);
} break;
case VOCAB_FEATURE2:
{
double value1, value2;
ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value1, &value2);
response.addDouble(value1);
response.addDouble(value2);
} break;
case VOCAB_ACTIVE:
{
bool _isActive;
ok = fgCtrl2->getActive(cmd.get(3).asInt(), &_isActive);
response.addInt(_isActive);
} break;
case VOCAB_MODE:
{
FeatureMode _mode;
ok = fgCtrl2->getMode(cmd.get(3).asInt(), &_mode);
response.addInt(_mode);
} break;
default:
{
yError() << "Unknown command 'GET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
response.clear();
ok = false;
}
} break; // end switch (param)
} break; // end VOCAB_GET
}
// yTrace() << "response is\n\t" << response.toString().c_str();
return ok;
}
void IFixedSizeMatrixInputLearner::writeBottle(yarp::os::Bottle& bot) {
bot.addInt(this->getDomainRows());
bot.addInt(this->getDomainCols());
bot.addInt(this->getCoDomainSize());
}
/*! @brief Fill a bottle with the contents of an object.
@param[in,out] aBottle The bottle to be filled.
@param[in] theData The value to be sent.
@param[in] hashMapFunction The function to be applied to hash tables to get values that can be
placed in a bottle.
@param[in] topLevel @c true if this is the outermost list of an object. */
static void
fillBottleFromValue(yarp::os::Bottle & aBottle,
cl_object theData,
cl_object hashMapFunction,
const bool topLevel)
{
ODL_ENTER(); //####
ODL_P3("aBottle = ", &aBottle, "theData = ", theData, "hashMapFunction = ", //####
hashMapFunction); //####
ODL_B1("topLevel = ", topLevel); //####
if (ECL_NIL != cl_integerp(theData))
{
ODL_LOG("(ECL_NIL != cl_integerp(theData))"); //####
aBottle.addInt(ecl_to_fixnum(theData));
}
else if (ECL_NIL != cl_realp(theData))
{
ODL_LOG("(ECL_NIL != cl_realp(theData))"); //####
aBottle.addDouble(ecl_to_double(theData));
}
else if (ECL_NIL != cl_stringp(theData))
{
ODL_LOG("(ECL_NIL != cl_stringp(theData))"); //####
cl_object aValue = si_coerce_to_base_string(theData);
if (ECL_NIL == aValue)
{
ODL_LOG("(ECL_NIL == aValue)"); //####
aBottle.addString("<unconvertible string>");
}
else
{
ODL_LOG("! (ECL_NIL == aValue)"); //####
aBottle.addString(reinterpret_cast<char *>(aValue->base_string.self));
}
}
else if (ECL_NIL != cl_symbolp(theData))
{
ODL_LOG("(ECL_NIL != cl_symbolp(theData))"); //####
cl_object aName = cl_symbol_name(theData);
if (ECL_NIL == aName)
{
ODL_LOG("(ECL_NIL == aName)"); //####
aBottle.addString("<problematic symbol>");
}
else
{
ODL_LOG("! (ECL_NIL == aName)"); //####
if (ECL_NIL == cl_stringp(aName))
{
ODL_LOG("(ECL_NIL == cl_stringp(aName))"); //####
aName = cl_string(aName);
}
aName = si_coerce_to_base_string(aName);
if (ECL_NIL == aName)
{
ODL_LOG("(ECL_NIL == aName)"); //####
aBottle.addString("<unconvertible symbol>");
}
else
{
ODL_LOG("! (ECL_NIL == aName)"); //####
aBottle.addString(reinterpret_cast<char *>(aName->base_string.self));
}
}
}
else if (ECL_NIL != cl_characterp(theData))
{
ODL_LOG("(ECL_NIL != cl_characterp(theData))"); //####
cl_object asString = cl_string(theData);
if (ECL_NIL == asString)
{
ODL_LOG("(ECL_NIL == asString)"); //####
aBottle.addString("<unconvertible character>");
}
else
{
ODL_LOG("! (ECL_NIL == asString)"); //####
aBottle.addString(reinterpret_cast<char *>(asString->base_string.self));
}
}
else if (ECL_NIL != cl_hash_table_p(theData))
{
ODL_LOG("(ECL_NIL != cl_hash_table_p(theData))"); //####
cl_env_ptr env = ecl_process_env();
cl_object aList;
cl_object errorSymbol = ecl_make_symbol("ERROR", "CL");
ECL_RESTART_CASE_BEGIN(env, ecl_list1(errorSymbol))
{
/* This form is evaluated with bound handlers. */
aList = cl_funcall(1, hashMapFunction, theData);
}
/*
* Message handler. RPC and from the terminal
*/
bool CoreModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
printDebug("Message Received: (echo is on)");
// debug
printf("GOT MESSAGE: %s\n", command.toString().c_str());
// QUIT
if( command.get(0).asString() == "quit" ||
command.get(0).asString() == "exit" ||
command.get(0).asString() == "stop" )
{
isRunning = false;
return false;
}
reply.clear();
reply.addString("The command is not valid! Try: quit|list|add|del");
// nothing there
if( command.size() < 1 ) return true;
if( command.size() == 1 && command.get(0).asString() == "") return true;
// LIST
if( command.get(0).asString() == "list" || command.get(0).asString() == "ls" ){
reply.clear();
reply.addString("list");
std::vector<ModuleInfo>::iterator itr;
for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
std::cout << (*itr).toStdString() << std::endl;
Bottle b;
(*itr).toBottle(b);
reply.addList() = b;
}
}
// ADDING A MODULE
if( command.get(0).asString() == "add" || command.get(0).asString() == "launch" ){
reply.clear();
if( command.size() < 2 ) {
reply.addString("ERROR: The syntax should be:");
reply.addString("add <name>");
return true;
}
int thisModuleID = nextModuleID++;
ModuleInfo i;
i.set(thisModuleID, command);
listOfModules.push_back(i);
reply.addString("OK");
reply.addInt(thisModuleID);
}
// DELETING A MODULE
if(command.get(0).asString() == "del" ||
command.get(0).asString() == "rm" ||
command.get(0).asString() == "delete") {
reply.clear();
if( command.size() < 2 ) {
reply.addString("ERROR: The syntax should be:");
reply.addString("del <moduleID>");
return true;
}
if( command.get(1).isInt() ) {
int thisModuleID = command.get(1).asInt();
reply.addString("OK");
// delete from vector
std::vector<ModuleInfo>::iterator itr;
for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
if(thisModuleID == (*itr).ID) {
listOfModules.erase(itr);
break;
}
}
} else {
reply.addString("ERROR: Could not parse integer! the syntax should be: del <moduleID as integer>");
}
}
// if( command.get(0).asString() == "set"){
//
// if( command.get(1).asString() == "tgt" || command.get(1).asString() == "target" ) {
// userSetTargetName = command.get(2).asString();
// reply.clear();
// if( userSetTargetName == "" ) {
// reply.addString("using latest object in the world as target!");
// } else {
// reply.addString(userSetTargetName.c_str());
// reply.addString(" is now the target!");
// }
//
//
// } else
//
// if( command.get(1).asString() == "offset" ) {
// if(reach) {
// if( command.get(2).isDouble() ) {
// reach->setOffset( command.get(2).asDouble() );
// reply.clear();
// reply.addString("OK");
// } else {
// reply.clear();
// reply.addString("ERROR: Could not parse double! the syntax should be: set offset <double>");
// }
// } else {
// reply.clear();
// reply.addString("ReachingWorker not ready yet!");
// }
// } else
//
// if( command.get(1).asString() == "policy" ) {
// if(reach) {
//
// if( command.get(2).asString() == "above" ) {
// reach->setPolicy( ReachingWorker::FROM_ABOVE | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "below" ) {
// reach->setPolicy( ReachingWorker::FROM_BELOW | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "left" ) {
// reach->setPolicy( ReachingWorker::FROM_LEFT | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else if( command.get(2).asString() == "right" ) {
// reach->setPolicy( ReachingWorker::FROM_RIGHT | ReachingWorker::STRAIGHT );
// reply.clear();
// reply.addString("OK");
//
// } else {
// reply.clear();
// reply.addString("ERROR: Could not parse policy! the syntax should be: set policy <above|below|left|right>");
// }
// } else {
// reply.clear();
// reply.addString("ReachingWorker not ready yet!");
// }
// } else {
//
// // not a correct command?!
// reply.clear();
// reply.addString("Could not parse command try set [tgt|policy|offset]!");
// }
return true;
}