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; }