bool ThreeDModule::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out, yarp::os::Stamp stamp) {
//? Stamp stamp;
std::cout << "responding: " << in.toString() << std::endl;
//TODO sanity check
//...
out.clear();
// process data "in", prepare "out"
out.append(in);
out.addList() = calculatePosition(in, stamp);
// reply
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;
}
