Exemple #1
0
 /**
 * Parser for VOCAB commands. It is called by virtual bool respond().
 * @param command the bottle containing the user command
 * @param reply the bottle which will be returned to the RPC client
 * @return true if the command was successfully parsed
 */
 bool parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
 {
     int request = command.get(1).asVocab();
     if (request == VOCAB_NAV_GET_CURRENT_POS)
     {
         //plannerThread->setNewAbsTarget(loc);
         reply.addVocab(VOCAB_OK);
         reply.addString(m_localization_data.map_id);
         reply.addDouble(m_localization_data.x);
         reply.addDouble(m_localization_data.y);
         reply.addDouble(m_localization_data.theta);
     }
     else if (request == VOCAB_NAV_SET_INITIAL_POS)
     {
         yarp::dev::Map2DLocation loc;
         loc.map_id = command.get(2).asString();
         loc.x = command.get(3).asDouble();
         loc.y = command.get(4).asDouble();
         loc.theta = command.get(5).asDouble();
         initializeLocalization(loc);
         reply.addVocab(VOCAB_OK);
     }
     else
     {
         reply.addVocab(VOCAB_ERR);
     }
     return true;
 }
bool WorldRpcInterface::handler( const yarp::os::Bottle& command, yarp::os::Bottle& reply ) 
{
	int n = 0;	// identifier of the current bottle element
	int cmd;	// the command (see command vocabs in header)
	
	yarp::os::ConstString prefix = command.get(n).asString();
	if ( prefix=="help" )
	{
			reply.addVocab(yarp::os::Vocab::encode("many"));
			reply.addString("\nMoBeE world interface, arguments within brackets: \n");
			reply.addString(	"ls: list objects");
			reply.addString(	"mk sph [radius] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"mk cyl [radius] [height] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"mk box [xsize] [ysize] [zsize] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"set [objectname] [xpos] [ypos] [zpos]: set object location (m)");
			reply.addString(	"def [objectname] [targ/obs]: set object class to target or obstacle");
			reply.addString(	"get [objectname]: return object state");
			reply.addString(	"rot [objectname] [xrot] [yrot] [zrot]: set object rotation (degrees)");
			reply.addString(    "rot [objectname] [1*9 rotation matrix]: set object rotation cosine matrix");
			reply.addString(	"rm [objectname]: remove object (persistent objects cannot be removed)");
			reply.addString(	"clr: remove all but persistent objects from the world, and reset object counters");
			reply.addString(	"grab [objectname] [robotname] [markername]: attach object to robot marker");
			reply.addString(	"grab [objectname] [robotname]: detach object from robot");
			reply.addString("\niCub simulator synchronization commands:");
			reply.addString(    "sim [objectname] [xpos] [ypos] [zpos] [xrot] [yrot] [zrot]: set rototranslation as returned in iCubSim coordinates");
			reply.addString(	"srun [period]: run iCubSim synchronization thread");
			reply.addString(	"sstp: stop iCubSim synchronization thread");
			reply.addString(	"sync: do one iCubSim synchronization step");
		return true;
	}
	else if ( prefix == "ls, mk (sph, cyl, box), set, def (obs/tgt), get, rot, rm, clr, grab, sim, srun, sstp, sync" ) { n++; }
	
	cmd  = command.get(n).asVocab(); n++;

	switch (cmd)
	{
		case VOCAB_LS:	getList(reply); break;
		case VOCAB_MK:	make(command,reply,n); break;
		case VOCAB_SET:	set(command,reply,n); break;
		case VOCAB_DEF: respClass(command,reply,n); break;
		case VOCAB_GET:	getState(command,reply,n); break;
		case VOCAB_ROT:	setRot(command,reply,n); break;
		case VOCAB_REM:	removeObject(command,reply,n); break;
		case VOCAB_GRAB: grabObject(command,reply,n); break;
		case VOCAB_SIM: setRTfromSim(command, reply, n); break;
		case VOCAB_SIMSYNC_RUN: startSimSyncer(command, reply, n); break;
		case VOCAB_SIMSYNC_STOP: model->getSimSyncer().stop(); reply.addString("ok"); break;
		case VOCAB_SIMSYNC_NOW: model->getSimSyncer().step(); reply.addString("ok"); break;
		case VOCAB_CLEAR: 
				printf("CLEARING THE WORLD\n");
				model->clearTheWorld();
				s = 0; c = 0; b = 0; ss = 0; sc = 0; sb = 0; // set counters to 0, for icub simulator compatibility
				reply.addString("Removed all world objects");
				printf("FINISHED CLEARING THE WORLD\n");
				break;
			
		default: reply.addString("Unknown RPC command"); return false;
	}
	return true;
}
Exemple #3
0
 /**
 * Parser for user command received from the RPC port
 * @param command the bottle containing the user command
 * @param reply the bottle which will be returned to the RPC client
 * @return true if the command was successfully parsed
 */
 virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) 
 {
     yarp::os::LockGuard lock(m_mutex);
     reply.clear(); 
     //parser for VOCAB  commands
     if (command.get(0).isVocab())
     {
         if(command.get(0).asVocab() == VOCAB_INAVIGATION && command.get(1).isVocab())
         {
             parse_respond_vocab(command,reply);
         }
         else
         {
             yError() << "Invalid vocab received";
             reply.addVocab(VOCAB_ERR);
         }
     }
     //parser for string commands
     else if (command.get(0).isString())
     {
         if (command.get(0).asString()=="help")
         {
             reply.addVocab(Vocab::encode("many"));
             reply.addString("Available commands are:");
             reply.addString("getLoc");
             reply.addString("initLoc <map_name> <x> <y> <angle in degrees>");
         }
         else if (command.get(0).isString())
         {
             parse_respond_string(command, reply);
         }
     }
     //unknown/invalid command received
     else
     {
         yError() << "Invalid command type";
         reply.addVocab(VOCAB_ERR);
     }
     return true;
 }
Exemple #4
0
 virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
 {
     reply.clear(); 
     if (command.get(0).isString())
     {
         if (command.get(0).asString()=="help")
         {
             reply.addVocab(Vocab::encode("many"));
             reply.addString("Available commands:");
             reply.addString("currently nothing");
             return true;
         }
         else if (command.get(0).asString()=="***")
         {
             return true;
         }
     }
     reply.addString("Unknown command");
     return true;
 }
bool Implement_DepthVisualParams_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ret = false;
    response.clear();
    if(!iDepthVisual)
    {
        yError() << "Depth Visual parameter Parser has not been correctly configured. IDepthVisualParams interface is not valid";
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    int code = cmd.get(0).asVocab();
    if(code != VOCAB_DEPTH_VISUAL_PARAMS)
    {
        yError() << "Depth Visual Params Parser received a command not belonging to this interface. Required interface was " << yarp::os::Vocab::decode(code);
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    switch (cmd.get(1).asVocab())
    {
        case VOCAB_GET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_HEIGHT:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_HEIGHT);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthHeight());
                }
                break;

                case VOCAB_WIDTH:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_WIDTH);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthWidth());
                }
                break;

                case VOCAB_FOV:
                {
                    double hFov, vFov;
                    ret = iDepthVisual->getDepthFOV(hFov, vFov);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_FOV);
                        response.addVocab(VOCAB_IS);
                        response.addDouble(hFov);
                        response.addDouble(vFov);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                    {
                        response.addVocab(VOCAB_FAILED);
                    }
                }
                break;

                case VOCAB_ACCURACY:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_ACCURACY);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(iDepthVisual->getDepthAccuracy());
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    double nearPlane, farPlane;
                    iDepthVisual->getDepthClipPlanes(nearPlane, farPlane);
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_CLIP_PLANES);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(nearPlane);
                    response.addDouble(farPlane);
                }
                break;

                case VOCAB_MIRROR:
                {
                    bool mirror;
                    ret = iDepthVisual->getDepthMirroring(mirror);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_MIRROR);
                        response.addVocab(VOCAB_IS);
                        response.addInt(mirror);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                default:
                {
                    yError() << "Depth Visual Parameter interface parser received am unknown GET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        case VOCAB_SET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_RESOLUTION:
                {
                    ret = iDepthVisual->setDepthResolution(cmd.get(3).asInt(), cmd.get(4).asInt());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_FOV:
                {
                    ret = iDepthVisual->setDepthFOV(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_ACCURACY:
                {
                    ret = iDepthVisual->setDepthAccuracy(cmd.get(3).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    ret = iDepthVisual->setDepthClipPlanes(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_MIRROR:
                {
                    ret = iDepthVisual->setDepthMirroring(cmd.get(3).asBool());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                default:
                {
                    yError() << "Rgb Visual Parameter interface parser received am unknown SET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        default:
        {
            yError() << "Rgb Visual parameter interface Parser received a malformed request. Command should either be 'set' or 'get', received " << cmd.toString();
            response.addVocab(VOCAB_FAILED);
            ret = false;
        }
        break;
    }
    return ret;
}
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);
}
Exemple #7
0
    virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
    {
        reply.clear(); 
        yDebug("receiving command from port\n");
        int index = 0;
        int cmdSize = command.size();
    
        while(cmdSize>0)
            {
                switch(command.get(index).asVocab())
                {
                    case  VOCAB4('s','u','s','p'):
                        reply.addVocab(Vocab::encode("ack"));
                        vc->halt();
                        cmdSize--;
                        index++;
                    break;
                    case VOCAB3('r','u','n'):
                        reply.addVocab(Vocab::encode("ack"));
                        vc->go();
                        cmdSize--;
                        index++;
                    break;
                    //this set current position reference
                    case VOCAB3('s','e','t'):
                        if (command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double pos=command.get(index+2).asDouble();
                                vc->setRef(i, pos);
                                index +=3;
                                cmdSize-=3;
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set message, ignoring\n");
                            }
                        reply.addVocab(Vocab::encode("ack"));
                    break;
                    //this set maximum velocity (limiter)
                    case VOCAB4('s','v','e','l'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double vel = command.get(index+2).asDouble();
                                vc->setVel(i,vel);
                                index += 3;
                                cmdSize-=3;;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set vel message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                    break;
                    case VOCAB4('g','a','i','n'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double gain = command.get(index+2).asDouble();
                                vc->setGain(i,gain);
                                index+=3;
                                cmdSize-=3;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set gain message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                    break;
                    case VOCAB4('h','e','l','p'):
                        fprintf(stdout,"VelocityControl module, valid commands are:\n");
                        fprintf(stdout,"-   [susp]         suspend the controller (command zero velocity)\n");
                        fprintf(stdout,"-   [run]          start (and resume after being suspended) the controller\n");
                        fprintf(stdout,"-   [quit]         quit the module (exit)\n");
                        fprintf(stdout,"-   [set]  <j> <p> move joint j to p (degrees)\n");
                        fprintf(stdout,"-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
                        fprintf(stdout,"-   [gain] <j> <k> set P gain for joint j to k\n");
                        fprintf(stdout,"-   [help] to get this help\n");
                        fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                        
                        reply.addVocab(Vocab::encode("many"));
                        reply.addVocab(Vocab::encode("ack"));
                        reply.addString("VelocityControl module, valid commands are:");
                        reply.addString("-   [susp]         suspend the controller (command zero velocity)");
                        reply.addString("-   [run]          start (and resume after being suspended) the controller");
                        reply.addString("-   [quit]         quit the module (exit)");
                        reply.addString("-   [set]  <j> <p> move joint j to p (degrees)");
                        reply.addString("-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
                        reply.addString("-   [gain] <j> <k> set P gain for joint j to k");
                        reply.addString("-   [help] to get this help");
                        reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                    break;
                    default:
                        yError("Invalid command, ignoring\n");
                        reply.addVocab(Vocab::encode("fail"));
                        //cmdSize--;
                        //index++;
                        //return respond(command, reply); // call default
                    break;
                }
                return true;
            }

        return false;
    }
Exemple #8
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->
}
Exemple #9
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->
}
Exemple #10
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;
    }
}
Exemple #11
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;
        }
}
// Callback handler for RPC commands (?)
bool RpcMsgHandler::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply)
{
    bool ok = true;
    bool commandUnderstood = true; // is the command recognized?

    if (caller->verbose())
        printf("command received: %s\n", cmd.toString().c_str());

    int code = cmd.get(0).asVocab();

    switch (code)
    {
        case VOCAB_GET:
        {
            double dtmp  = 0.0;
            double dtmp2 = 0.0;
            reply.addVocab(VOCAB_IS);
            reply.add(cmd.get(1));

            switch(cmd.get(1).asVocab())
            {
                case VOCAB_AXES:
                {
                    int axes;
                    caller->getAxes(axes);
                    reply.addInt(axes);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    ok = caller->getDebugParameter(j, index, &dtmp);
                    reply.addInt(j);
                    reply.addInt(index);
                    reply.addDouble(dtmp);
                }
                break;

                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    ok = caller->getParameter(j, param, &dtmp);
                    reply.addInt(j);
                    reply.addInt(param);
                    reply.addDouble(dtmp);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 1: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_GET

        case VOCAB_SET:
        {
            switch(cmd.get(1).asVocab())
            {
                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setParameter(j, param, val);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setDebugParameter(j, index, val);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 2: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_SET

        default:
        {
            commandUnderstood = false;
            std::cout << "Debug Interface 3: command not understood! received " << cmd.toString().c_str() << std::endl;
        }
        break;

    } //switch code

    if (!commandUnderstood)
    {
        ok = DeviceResponder::respond(cmd,reply);
    }


    if (!ok)
    {
        // failed thus send only a VOCAB back.
        reply.clear();
        reply.addVocab(VOCAB_FAILED);
    }
    else
        reply.addVocab(VOCAB_OK);

    return ok;
}
bool FrameGrabberControls2_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ok = false;
    int action = cmd.get(1).asVocab();
    int param  = cmd.get(2).asVocab();

//     yTrace() << "cmd received\n\t" << cmd.toString().c_str();


    if(!fgCtrl2)
    {
        yError() << " Selected camera device has no IFrameGrabberControl2 interface";
        return false;
    }

    response.clear();

    switch (action)
    {
        case VOCAB_HAS:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(VOCAB_HAS);
            response.addVocab(VOCAB_FEATURE);
            response.addInt(param);

            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    bool _hasFeat;
                    ok = fgCtrl2->hasFeature(cmd.get(3).asInt(), &_hasFeat);
                    response.addInt(_hasFeat);
                } break;

                case VOCAB_ONOFF:
                {
                    bool _hasOnOff;
                    ok = fgCtrl2->hasOnOff(cmd.get(3).asInt(), &_hasOnOff);
                    response.addInt(_hasOnOff);
                } break;

                case VOCAB_AUTO:
                {
                    bool _hasAuto;
                    ok = fgCtrl2->hasAuto(cmd.get(3).asInt(), &_hasAuto);
                    response.addInt(_hasAuto);
                } break;

                case VOCAB_MANUAL:
                {
                    bool _hasManual;
                    ok = fgCtrl2->hasManual(cmd.get(3).asInt(), &_hasManual);
                    response.addInt(_hasManual);
                } break;

                case VOCAB_ONEPUSH:
                {
                    bool _hasOnePush;
                    ok = fgCtrl2->hasOnePush(cmd.get(3).asInt(), &_hasOnePush);
                    response.addInt(_hasOnePush);
                } break;

                default:
                {
                    yError() << "Unknown command 'HAS " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                } break;
            } break; // end switch (param)

        } break; // end VOCAB_HAS

        case VOCAB_SET:
        {
            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble());
                } break;

                case VOCAB_FEATURE2:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble(), cmd.get(5).asDouble());
                } break;

                case VOCAB_ACTIVE:
                {
                    ok = fgCtrl2->setActive(cmd.get(3).asInt(), cmd.get(4).asInt());
                } break;

                case VOCAB_MODE:
                {
                    ok = fgCtrl2->setMode(cmd.get(3).asInt(), (FeatureMode) cmd.get(4).asInt());
                } break;

                case VOCAB_ONEPUSH:
                {
                    ok = fgCtrl2->setOnePush(cmd.get(3).asInt());
                } break;

                default:
                {
                    yError() << "Unknown command 'SET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }
            } break; // end switch (param)

        } break; // end VOCAB_SET

        case VOCAB_GET:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(param);
            response.addVocab(VOCAB_IS);
            switch (param)
            {
                case VOCAB_CAMERA_DESCRIPTION:
                {
                    CameraDescriptor camera;
                    ok = fgCtrl2->getCameraDescription(&camera);
                    response.addInt(camera.busType);
                    response.addString(camera.deviceDescription);
                    yDebug() << "Response is " << response.toString();
                } break;

                case VOCAB_FEATURE:
                {
                    double value;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value);
                    response.addDouble(value);
                } break;

                case VOCAB_FEATURE2:
                {
                    double value1, value2;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value1, &value2);
                    response.addDouble(value1);
                    response.addDouble(value2);
                } break;

                case VOCAB_ACTIVE:
                {
                    bool _isActive;
                    ok = fgCtrl2->getActive(cmd.get(3).asInt(), &_isActive);
                    response.addInt(_isActive);
                } break;

                case VOCAB_MODE:
                {
                    FeatureMode _mode;
                    ok = fgCtrl2->getMode(cmd.get(3).asInt(), &_mode);
                    response.addInt(_mode);
                } break;

                default:
                {
                    yError() << "Unknown command 'GET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }

            } break; // end switch (param)

        } break; // end VOCAB_GET
    }
//     yTrace() << "response is\n\t" << response.toString().c_str();
    return ok;
}
bool JoypadCtrlParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ret;

    ret = false;
    if(cmd.get(0).asVocab() != VOCAB_IJOYPADCTRL || !cmd.get(1).isVocab() || !cmd.get(2).isVocab() || !cmd.get(3).isVocab())
    {
        response.addVocab(VOCAB_FAILED);
        return ret;
    }

    if(cmd.get(1).asVocab() == VOCAB_GET)
    {
        int toGet;

        toGet = cmd.get(2).asVocab();

        if(cmd.get(3).asVocab() == VOCAB_COUNT)
        {
            if(countGetters.find(toGet) != countGetters.end())
            {
                unsigned int   count;
                getcountmethod getter;
                getter = countGetters[toGet];
                if((device->*getter)(count))
                {
                    response.addVocab(VOCAB_OK);
                    response.addInt32(count);
                    ret = true;
                }
            }
            else if (toGet == VOCAB_STICKDOF && cmd.get(4).isInt32())
            {
                unsigned int count;
                if (device->getStickDoF(cmd.get(4).asInt32(), count))
                {
                    response.addVocab(VOCAB_OK);
                    response.addInt32(count);
                    ret = true;
                }
                else
                {
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
            }
            else
            {
                response.addVocab(VOCAB_FAILED);
                ret = false;
            }
        }
        else if(cmd.get(3).asVocab() == VOCAB_VALUE)
        {
            switch (cmd.get(2).asVocab()) {
            case VOCAB_BUTTON:
            {
                float value;
                if(cmd.get(4).isInt32() && device->getButton(cmd.get(4).asInt32(), value))
                {
                    response.addVocab(VOCAB_OK);
                    response.addFloat64(value);
                    ret = true;
                }
                break;
            }
            case VOCAB_AXIS:
            {
                double value;
                if(cmd.get(4).isInt32() && device->getAxis(cmd.get(4).asInt32(), value))
                {
                    response.addVocab(VOCAB_OK);
                    response.addFloat64(value);
                    ret = true;
                }
                break;
            }
            case VOCAB_STICK:
            {
                if(cmd.get(4).isVocab())
                {
                    yarp::sig::Vector frame;

                    auto mode = cmd.get(4).asVocab() == VOCAB_CARTESIAN ? yarp::dev::IJoypadController::JypCtrlcoord_CARTESIAN : yarp::dev::IJoypadController::JypCtrlcoord_POLAR;
                    if(cmd.get(5).isInt32() && device->getStick(cmd.get(5).asInt32(), frame, mode))
                    {
                        response.addVocab(VOCAB_OK);
                        for(size_t i = 0; i < frame.size(); ++i)
                        {
                            response.addFloat64(frame[i]);
                        }

                        ret = true;
                    }
                }
                break;
            }
            case VOCAB_STICKDOF:
            {

                unsigned int dofCount;

                if(cmd.get(5).isInt32() && device->getStickDoF(cmd.get(5).asInt32(), dofCount))
                {
                    response.addVocab(VOCAB_OK);
                    response.addInt32(dofCount);
                    ret = true;
                }

                break;
            }
            case VOCAB_TOUCH:
            {
                yarp::sig::Vector pos;
                unsigned int      id;

                id = cmd.get(4).asInt32();
                if(cmd.get(4).isInt32() && device->getTouch(id, pos))
                {
                    response.addVocab(VOCAB_OK);
                    for(size_t i = 0; i < pos.size(); ++i)
                    {
                        response.addFloat64(pos[i]);
                    }
                    ret = true;
                }
                break;
            }
            case VOCAB_TRACKBALL:
            {
                yarp::sig::Vector axes;
                unsigned int      id;

                id = cmd.get(4).asInt32();
                if(cmd.get(4).isInt32() && device->getTrackball(id, axes))
                {
                    response.addVocab(VOCAB_OK);
                    for(size_t i = 0; i < axes.size(); ++i)
                    {
                        response.addFloat64(axes[i]);
                    }
                    ret = true;
                }
                break;
            }
            case VOCAB_HAT:
            {
                unsigned char value;
                if(cmd.get(4).isInt32() && device->getHat(cmd.get(4).asInt32(), value))
                {
                    response.addVocab(VOCAB_OK);
                    response.addInt32(value);
                    ret = true;
                }
                break;
            }
            default:
                break;
            }
        }
    }
    return ret;

}
    virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
    {
        fprintf(stderr,"receiving command from port\n");
        int index = 0;
        int cmdSize = command.size();
    
        while(cmdSize>0)
            {
                switch(command.get(index).asVocab())  {
                case  VOCAB4('s','u','s','p'):
                    {
                        reply.addVocab(Vocab::encode("ack"));
                        vc->halt();
                        cmdSize--;
                        index++;
                        break;
                    }
                case VOCAB3('r','u','n'):
                    {
                        reply.addVocab(Vocab::encode("ack"));
                        vc->go();
                        cmdSize--;
                        index++;
                        break;
                    }
                case VOCAB3('s','e','t'):
                    {
                        if (command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double pos=command.get(index+2).asDouble();
                                vc->setRef(i, pos);
                                index +=3;
                                cmdSize-=3;
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                fprintf(stderr, "Invalid set message, ignoring\n");
                            }
                        reply.addVocab(Vocab::encode("ack"));
                        break;
                    }
                case VOCAB4('s','v','e','l'):
                    {
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double vel = command.get(index+2).asDouble();
                                vc->setVel(i,vel);
                                index += 3;
                                cmdSize-=3;;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                fprintf(stderr,"Invalid set vel message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                        break;
                    }
                case VOCAB4('g','a','i','n'):
                    {
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double gain = command.get(index+2).asDouble();
                                vc->setGain(i,gain);
                                index+=3;
                                cmdSize-=3;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                fprintf(stderr,"Invalid set gain message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                        break;
                    }
                default:
                    {
                        cmdSize--;
                        index++;
                        return RFModule::respond(command, reply); // call default
                    }
                }
                return true;
            }

        return false;
    }
Exemple #16
0
    virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) 
    {
        reply.clear(); 

        gotoThread->mutex.wait();
        if (command.get(0).asString()=="quit")
        {
            gotoThread->mutex.post();
            return false;
        }

        else if (command.get(0).asString()=="help")
        {
            reply.addVocab(Vocab::encode("many"));
            reply.addString("Available commands are:");
            reply.addString("gotoAbs <x> <y> <angle>");
            reply.addString("gotoRel <x> <y> <angle>");
            reply.addString("stop");
            reply.addString("pause");
            reply.addString("resume");
            reply.addString("quit");
            reply.addString("set linear_tol <m>");
            reply.addString("set linear_ang <deg>");
            reply.addString("set max_lin_speed <m/s>");
            reply.addString("set max_ang_speed <deg/s>");
            reply.addString("set min_lin_speed <m/s>");
            reply.addString("set min_ang_speed <deg/s>");
            reply.addString("set obstacle_stop");
            reply.addString("set obstacle_avoidance");
        }

        else if (command.get(0).asString()=="gotoAbs")
        {
            yarp::sig::Vector v;
            v.push_back(command.get(1).asDouble());
            v.push_back(command.get(2).asDouble());
            if (command.size()==4) v.push_back(command.get(3).asDouble());
            gotoThread->setNewAbsTarget(v);
            reply.addString("new absolute target received");
        }

        else if (command.get(0).asString()=="gotoRel")
        {
            yarp::sig::Vector v;
            v.push_back(command.get(1).asDouble());
            v.push_back(command.get(2).asDouble());
            if (command.size()==4) v.push_back(command.get(3).asDouble());
            gotoThread->setNewRelTarget(v);
            reply.addString("new relative target received");
        }
        else if (command.get(0).asString()=="set")
        {
            if (command.get(1).asString()=="linear_tol")
            {
                gotoThread->goal_tolerance_lin=command.get(2).asDouble();
                reply.addString("linear_tol set.");
            }
            else if (command.get(1).asString()=="angular_tol")
            {
                gotoThread->goal_tolerance_ang=command.get(2).asDouble();
                reply.addString("angular_tol set.");
            }
            else if (command.get(1).asString()=="max_lin_speed")
            {
                gotoThread->max_lin_speed=command.get(2).asDouble();
                reply.addString("max_lin_speed set.");
            }
            else if (command.get(1).asString()=="max_ang_speed")
            {
                gotoThread->max_ang_speed=command.get(2).asDouble();
                reply.addString("max_ang_speed set.");
            }
            else if (command.get(1).asString()=="min_lin_speed")
            {
                gotoThread->min_lin_speed=command.get(2).asDouble();
                reply.addString("min_lin_speed set.");
            }
            else if (command.get(1).asString()=="min_ang_speed")
            {
                gotoThread->min_ang_speed=command.get(2).asDouble();
                reply.addString("min_ang_speed set.");
            }
            else if (command.get(1).asString()=="obstacle_avoidance")
            {
                if (gotoThread->enable_obstacles_avoidance)
                    {
                        reply.addString("enable_obstacles_avoidance=false");
                        gotoThread->enable_obstacles_avoidance=false;
                    }
                else
                    {
                        gotoThread->enable_obstacles_avoidance=true;
                        reply.addString("enable_obstacles_avoidance=true");
                    }
            }
            else if (command.get(1).asString()=="obstacle_stop")
            {
                if (gotoThread->enable_obstacles_avoidance)
                    {
                        reply.addString("enable_obstacle_stop=false");
                        gotoThread->enable_obstacles_emergency_stop=false;
                    }
                else
                    {
                        gotoThread->enable_obstacles_emergency_stop=true;
                        reply.addString("enable_obstacle_stop=true");
                    }
            }
            else
            {
                reply.addString("Unknown set.");
            }
        }
        else if (command.get(0).asString()=="get")
        {
            if (command.get(1).asString()=="navigation_status")
            {
                string s = gotoThread->getNavigationStatus();
                reply.addString(s.c_str());
            }
            else
            {
                reply.addString("Unknown get.");
            }
        }
        else if (command.get(0).asString()=="stop")
        {
            gotoThread->stopMovement();
            reply.addString("Stopping movement.");
        }
        else if (command.get(0).asString()=="pause")
        {
            double time = -1;
            if (command.size() > 1)
                time = command.get(1).asDouble();
            gotoThread->pauseMovement(time);
            reply.addString("Pausing.");
        }
        else if (command.get(0).asString()=="resume")
        {
            gotoThread->resumeMovement();
            reply.addString("Resuming.");
        }
        else
        {
            reply.addString("Unknown command.");
        }

        gotoThread->mutex.post();
        return true;
    }