/** * Parser for VOCAB commands. It is called by virtual bool respond(). * @param command the bottle containing the user command * @param reply the bottle which will be returned to the RPC client * @return true if the command was successfully parsed */ bool parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply) { int request = command.get(1).asVocab(); if (request == VOCAB_NAV_GET_CURRENT_POS) { //plannerThread->setNewAbsTarget(loc); reply.addVocab(VOCAB_OK); reply.addString(m_localization_data.map_id); reply.addDouble(m_localization_data.x); reply.addDouble(m_localization_data.y); reply.addDouble(m_localization_data.theta); } else if (request == VOCAB_NAV_SET_INITIAL_POS) { yarp::dev::Map2DLocation loc; loc.map_id = command.get(2).asString(); loc.x = command.get(3).asDouble(); loc.y = command.get(4).asDouble(); loc.theta = command.get(5).asDouble(); initializeLocalization(loc); reply.addVocab(VOCAB_OK); } else { reply.addVocab(VOCAB_ERR); } return true; }
bool yarp::dev::ServerInertial::getInertial(yarp::os::Bottle &bot) { if (IMU==NULL) { return false; } else { int nchannels; IMU->getChannels (&nchannels); yarp::sig::Vector indata(nchannels); bool worked(false); worked=IMU->read(indata); if (worked) { bot.clear(); // Euler+accel+gyro+magn orientation values for (int i = 0; i < nchannels; i++) bot.addDouble (indata[i]); } else { bot.clear(); //dummy info. } return(worked); } }
void iCub::skinDynLib::vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b) { for (unsigned int i = 0; i < v.size(); i++) { b.addDouble(v[i]); } }
void iCub::skinDynLib::matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b) { Vector v = toVector(m); for (unsigned int i = 0; i < v.size(); i++) { b.addDouble(v[i]); } }
void DoubleArgumentDescriptor::addValueToBottle (yarp::os::Bottle & container) { ODL_ENTER(); //#### ODL_P1("container = ", &container); //#### container.addDouble(_currentValue); ODL_EXIT(); //#### } // DoubleArgumentDescriptor::addValueToBottle
void WorldRpcInterface::getState( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n ) { KinematicModel::CompositeObject* object = getObject( command, reply, n ); if ( object ) { const qreal* T = object->getT().data(); for ( int i = 0; i < 16; i++ ) { reply.addDouble( T[i] ); } //reply.addString("Set rotation (about x,y,z in degrees) of object."); } }
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; }
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ void iCub::skinForceControl::addToBottle(yarp::os::Bottle &b, const yarp::sig::Vector &v) throw() { for(unsigned int i=0; i<v.size(); i++) b.addDouble(v[i]); }
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 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-> }
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
// 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; }
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; }
/*! @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); }