Esempio n. 1
0
/* 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;
}
Esempio n. 2
0
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);
}
Esempio n. 4
0
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;
    }
}
Esempio n. 5
0
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
Esempio n. 7
0
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;
        }
}
Esempio n. 8
0
 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;
 }
Esempio n. 9
0
// 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;


        }
    }
}
Esempio n. 11
0
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);
        }
Esempio n. 14
0
/*
* 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;
}