Example #1
0
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->
}
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);
}
Example #3
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->
}
// 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;
}
Example #5
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;
 }
bool DispatcherManager::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply) {
    bool success = false;

    try {
        switch(cmd.get(0).asVocab()) {
            case VOCAB4('h','e','l','p'): // print help information
                reply.add(yarp::os::Value::makeVocab("help"));

                reply.addString("Event Manager configuration options");
                reply.addString("  help                  Displays this message");
                reply.addString("  list                  Print a list of available event listeners");
                reply.addString("  add type [type2 ...]  Adds one or more event listeners");
                reply.addString("  remove [all|idx]      Removes event listener at an index or all");
                reply.addString("  set [all|idx]         Configures a listener");
                reply.addString("  stats                 Prints information");
                success = true;
                break;

            case VOCAB4('l','i','s','t'): // print list of available event listeners
                {
                reply.add(yarp::os::Value::makeVocab("help"));
                std::vector<std::string> keys = FactoryT<std::string, IEventListener>::instance().getKeys();
                for(unsigned int i = 0; i < keys.size(); i++) {
                    reply.addString((std::string("  ") + keys[i]).c_str());
                }
                success = true;
                break;
                }

            case VOCAB3('a','d','d'): // add
                { // prevent identifier initialization to cross borders of case
                yarp::os::Bottle list = cmd.tail();
                for(int i = 0; i < list.size(); i++) {
                    IEventListener* listener = this->factory->create(list.get(i).asString().c_str());
                    listener->start();
                    this->dispatcher->addListener(listener);
                }
                reply.addString("Successfully added listener(s)");
                success = true;
                break;
                }

            case VOCAB4('r','e','m','o'): // remove
            case VOCAB3('d','e','l'): // del(ete)
                { // prevent identifier initialization to cross borders of case
                if(cmd.get(1).isInt() && cmd.get(1).asInt() >= 1 && cmd.get(1).asInt() <= this->dispatcher->countListeners()) {
                    this->dispatcher->removeListener(cmd.get(1).asInt()-1);
                    reply.addString("Successfully removed listener.");
                    success = true;
                } else if(cmd.get(1).asString() == "all") {
                    this->dispatcher->clear();
                    reply.addString("Successfully removed all listeners.");
                    success = true;
                } else {
                    throw std::runtime_error("Illegal index!");
                }
                break;
                }

            case VOCAB3('s','e','t'): // set
                { // prevent identifier initialization to cross borders of case
                yarp::os::Bottle property;
                property.addList() = cmd.tail().tail(); // see comment in TrainModule

                std::string replymsg = "Setting configuration option ";
                if(cmd.get(1).isInt() && cmd.get(1).asInt() >= 1 &&
                   cmd.get(1).asInt() <= this->dispatcher->countListeners()) {

                    bool ok = this->dispatcher->getAt(cmd.get(1).asInt()-1).configure(property);
                    replymsg += ok ? "succeeded" :
                                     "failed; please check key and value type.";
                    reply.addString(replymsg.c_str());
                    success = true;
                } else if(cmd.get(1).asString() == "all") {
                    for(int i = 0; i < this->dispatcher->countListeners(); i++) {
                        if(i > 0) {
                            replymsg += ", ";
                        }
                        bool ok = this->dispatcher->getAt(i).configure(property);
                        replymsg += ok ? "succeeded" :
                                        "failed; please check key and value type.";
                    }
                    replymsg += ".";
                    reply.addString(replymsg.c_str());
                    success = true;
                } else {
                    throw std::runtime_error("Illegal index!");
                }
                break;
                }

            case VOCAB4('i','n','f','o'): // information
            case VOCAB4('s','t','a','t'): // statistics
                { // prevent identifier initialization to cross borders of case
                reply.add(yarp::os::Value::makeVocab("help"));
                std::ostringstream buffer;
                buffer << "Event Manager Information (" << this->dispatcher->countListeners() << " listeners)";
                reply.addString(buffer.str().c_str());
                for(int i = 0; i < this->dispatcher->countListeners(); i++) {
                    buffer.str(""); // why isn't there a proper reset method?
                    buffer << "  [" << (i + 1) << "] " << this->dispatcher->getAt(i).getInfo();
                    reply.addString(buffer.str().c_str());
                }

                success = true;
                break;
                }

            default:
                break;

        }
    } catch(const std::exception& e) {
        std::string msg = std::string("Error: ") + e.what();
        reply.addString(msg.c_str());
        success = true;
    }

    return success;
}
Example #7
0
bool TrainModule::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply) {
    // NOTE: the module class spawns a new thread, which implies that exception
    // handling needs to be done in this thread, so not the 'main' thread.
    bool success = false;

    try {
        switch(cmd.get(0).asVocab()) {
            case VOCAB4('h','e','l','p'): // print help information
                reply.add(yarp::os::Value::makeVocab("help"));

                reply.addString("Training module configuration options");
                reply.addString("  help                  Displays this message");
                reply.addString("  train                 Trains the machine and sends the model");
                reply.addString("  model                 Sends the model to the prediction module");
                reply.addString("  reset                 Resets the machine to its current state");
                reply.addString("  info                  Outputs information about the machine");
                reply.addString("  pause                 Disable passing the samples to the machine");
                reply.addString("  continue              Enable passing the samples to the machine");
                reply.addString("  set key val           Sets a configuration option for the machine");
                reply.addString("  load fname            Loads a machine from a file");
                reply.addString("  save fname            Saves the current machine to a file");
                reply.addString("  event [cmd ...]       Sends commands to event dispatcher (see: event help)");
                reply.addString("  cmd fname             Loads commands from a file");
                reply.addString(this->getMachine().getConfigHelp().c_str());
                success = true;
                break;

            case VOCAB4('t','r','a','i'): // train the machine, implies sending model
                this->getMachine().train();
                reply.addString("Training completed.");

            case VOCAB4('m','o','d','e'): // send model
                this->model_out.write(this->machinePortable);
                reply.addString("The model has been written to the port.");
                success = true;
                break;

            case VOCAB4('c','l','e','a'): // clear machine
            case VOCAB3('c','l','r'):
            case VOCAB4('r','e','s','e'): // reset
            case VOCAB3('r','s','t'):
                this->getMachine().reset();
                reply.addString("Machine cleared.");
                success = true;
                break;

            case VOCAB4('p','a','u','s'): // pause sample stream
            case VOCAB4('d','i','s','a'): // disable
                this->trainProcessor.setEnabled(false);
                reply.addString("Sample stream to machine disabled.");
                success = true;
                break;

            case VOCAB4('c','o','n','t'): // continue sample stream
            case VOCAB4('e','n','a','b'): // enable
                this->trainProcessor.setEnabled(true);
                reply.addString("Sample stream to machine enabled.");
                success = true;
                break;

            case VOCAB4('i','n','f','o'): // information
            case VOCAB4('s','t','a','t'): // statistics
                { // prevent identifier initialization to cross borders of case
                reply.add(yarp::os::Value::makeVocab("help"));
                reply.addString("Machine Information: ");
                reply.addString(this->getMachine().getInfo().c_str());
                success = true;
                break;
                }

            case VOCAB4('l','o','a','d'): // load
                { // prevent identifier initialization to cross borders of case
                reply.add(yarp::os::Value::makeVocab("help"));
                std::string replymsg = std::string("Loading machine from '") +
                                       cmd.get(1).asString().c_str() + "'... " ;
                if(!cmd.get(1).isString()) {
                    replymsg += "failed";
                } else {
                    this->getMachinePortable().readFromFile(cmd.get(1).asString().c_str());
                    replymsg += "succeeded";
                }
                reply.addString(replymsg.c_str());
                success = true;
                break;
                }

            case VOCAB4('s','a','v','e'): // save
                { // prevent identifier initialization to cross borders of case
                reply.add(yarp::os::Value::makeVocab("help"));
                std::string replymsg = std::string("Saving machine to '") +
                                       cmd.get(1).asString().c_str() + "'... " ;
                if(!cmd.get(1).isString()) {
                    replymsg += "failed";
                } else {
                    this->getMachinePortable().writeToFile(cmd.get(1).asString().c_str());
                    replymsg += "succeeded";
                }
                reply.addString(replymsg.c_str());
                success = true;
                break;
                }

            case VOCAB3('s','e','t'): // set a configuration option for the machine
                { // prevent identifier initialization to cross borders of case
                yarp::os::Bottle property;
                /*
                 * This is a simple hack to enable multiple parameters The need
                 * for this hack lies in the fact that a group can only be found
                 * using findGroup if it is a nested list in a Bottle. If the
                 * Bottle itself is the list, then the group will _not_ be found.
                 */
                property.addList() = cmd.tail();
                std::string replymsg = "Setting configuration option ";
                bool ok = this->getMachine().configure(property);
                replymsg += ok ? "succeeded" :
                                 "failed; please check key and value type.";
                reply.addString(replymsg.c_str());
                success = true;
                break;
                }

            case VOCAB4('e','v','e','n'): // event
                { // prevent identifier initialization to cross borders of case
                success = this->dmanager.respond(cmd.tail(), reply);
                break;
                }

            case VOCAB3('c','m','d'): // cmd
            case VOCAB4('c','o','m','m'): // command
                { // prevent identifier initialization to cross borders of case
                reply.add(yarp::os::Value::makeVocab("help"));
                std::string replymsg;
                if(!cmd.get(1).isString()) {
                    replymsg = "Please supply a valid filename.";
                } else {
                    std::string full_fname = this->findFile(cmd.get(1).asString().c_str());
                    replymsg = std::string("Loading commands from '") +
                                           full_fname + "'... " ;
                    this->loadCommandFile(full_fname, &reply);
                    replymsg += "succeeded";
                }
                reply.addString(replymsg.c_str());
                success = true;
                break;
                }

            default:
                break;

        }
    } catch(const std::exception& e) {
        std::string msg = std::string("Error: ") + e.what();
        reply.addString(msg.c_str());
        success = true;
    }

    return success;
}