C++ PolyDriver类代码示例

bool DeviceGroup::open(const char *key, PolyDriver& poly,
                       yarp::os::Searchable& config, const char *comment) {

    Value *name;
    if (config.check(key,name,comment)) {
        if (name->isString()) {
            // maybe user isn't doing nested configuration
            yarp::os::Property p;
            p.setMonitor(config.getMonitor(),
                         name->toString().c_str()); // pass on any monitoring
            p.fromString(config.toString());
            p.put("device",name->toString());
            p.unput("subdevice");
            p.unput("wrapped");
            poly.open(p);
        } else {
            Bottle subdevice = config.findGroup(key).tail();
            poly.open(subdevice);
        }
        if (!poly.isValid()) {
            printf("cannot make <%s>\n", name->toString().c_str());
            return false;
        }
    } else {
        printf("\"--%s <name>\" not set\n", key);
        return false;
    }
    return true;
}

 bool add(const char *name, yarp::os::Searchable& config) {
     //printf("ADDING %s\n", config.toString().c_str());
     PolyDriver *pd = new PolyDriver();
     YARP_ASSERT (pd!=NULL);
     bool result = pd->open(config);
     if (!result) {
         delete pd;
         return false;
     }
     drivers.push_back(pd);
     names.push_back(ConstString(name));
     IService *service = NULL;
     pd->view(service);
     bool backgrounded = true;
     if (service!=NULL) {
         backgrounded = service->startService();
         if (backgrounded) {
             // we don't need to poll this, so forget about the
             // service interface
             printf("group: service backgrounded\n");
             service = NULL;
         }
     }
     needDrive.push_back(!backgrounded);
     needDriveSummary = needDriveSummary || (!backgrounded);
     Drivers::factory().add(new DriverLinkCreator(name,*pd));
     return true;
 }

    virtual bool threadInit()
    {
        // open a client interface to connect to the gaze server
        // we suppose that:
        // 1 - the iCub simulator (icubSim) is running
        // 2 - the gaze server iKinGazeCtrl is running and
        //     launched with --robot icubSim option
        Property optGaze("(device gazecontrollerclient)");
        optGaze.put("remote","/iKinGazeCtrl");
        optGaze.put("local","/gaze_client");

        clientGaze=new PolyDriver;
        if (!clientGaze->open(optGaze))
        {
            delete clientGaze;    
            return false;
        }

        // open the view
        clientGaze->view(igaze);

        // set trajectory time:
        // we'll go like hell since we're using the simulator :)
        igaze->setNeckTrajTime(0.4);
        igaze->setEyesTrajTime(0.1);

        // put the gaze in tracking mode, so that
        // when the torso moves, the gaze controller 
        // will compensate for it
/*pramod modified starts
        igaze->setTrackingMode(true);

        Property optTorso("(device remote_controlboard)");
        optTorso.put("remote","/icubSim/torso");
        optTorso.put("local","/torso_client");

        clientTorso=new PolyDriver;
        if (!clientTorso->open(optTorso))
        {
            delete clientTorso;    
            return false;
        }

        // open the view
        clientTorso->view(ienc);
        clientTorso->view(ipos);
        ipos->setRefSpeed(0,10.0);
pramod modified ends */
        fp.resize(3);

        state=STATE_TRACK;

		t=t0=t1=t2=t3=t4=Time::now();

        return true;
    }

 Gazer(PolyDriver &driver, ActionPrimitives *action, const Vector &offset) : 
       RateThread(50)
 {
     driver.view(this->igaze);
     this->action=action;
     this->offset=offset;
 }

int main(int argc, char *argv[])
{
    Network yarp;       // Initialize yarp framework

    // use YARP to create and configure an instance of fakeDepthCamera
    Property config;
    config.put("device", "fakeDepthCamera");         // device producing (fake) data
    config.put("mode",   "ball");                    // fake data type to be produced

    PolyDriver dd;
    dd.open(config);
    if (!dd.isValid())
    {
        yError("Failed to create and configure a the fake device\n");
        return 1;
    }

    yarp::dev::IRGBDSensor *RGBDInterface;              // interface we want to use
    if (!dd.view(RGBDInterface))                        // grab wanted device interface
    {
        yError("Failed to get RGBDInterface device interface\n");
        return 1;
    }

    // Let's use the interface to get info from device
    int rgbImageHeight   = RGBDInterface->getRgbHeight();
    int rgbImageWidth    = RGBDInterface->getRgbWidth();
    int depthImageHeight = RGBDInterface->getDepthHeight();
    int depthImageWidth  = RGBDInterface->getDepthWidth();

    FlexImage rgbImage;
    ImageOf<PixelFloat> depthImage;
    bool gotImage = RGBDInterface->getImages(rgbImage, depthImage);

    if(gotImage)
        yInfo("Succesfully retieved an image");
    else
        yError("Failed retieving images");

    yarp::os::Property intrinsic;
    RGBDInterface->getRgbIntrinsicParam(intrinsic);
    yInfo("RGB intrinsic parameters: \n%s", intrinsic.toString().c_str());
    dd.close();

    return 0;
}

    bool connect()
    {
        drv=new PolyDriver(drvOptions);

        if (drv->isValid())
            connected=drv->view(pos)&&drv->view(enc);
        else
            connected=false;

        if (!connected)
        {
            delete drv;
            drv=0;
        }

        return connected;
    }

 void testBasic() {
     report(0,"a very basic driver instantiation test");
     PolyDriver dd;
     Property p;
     p.put("device","test_grabber");
     bool result;
     result = dd.open(p);
     checkTrue(result,"open reported successful");
     IFrameGrabberImage *grabber = NULL;
     result = dd.view(grabber);
     checkTrue(result,"interface reported");
     ImageOf<PixelRgb> img;
     grabber->getImage(img);
     checkTrue(img.width()>0,"interface seems functional");
     result = dd.close();
     checkTrue(result,"close reported successful");
 }

bool OnlineStictionEstimator::configure(PolyDriver &driver, const Property &options)
{
    if (driver.isValid() && options.check("joint"))
    {
        bool ok=true;
        ok&=driver.view(imod);
        ok&=driver.view(ilim);
        ok&=driver.view(ienc);
        ok&=driver.view(ipid);
        ok&=driver.view(iolc);

        if (!ok)
            return false;

        joint=options.find("joint").asInt();
        setRate((int)(1000.0*options.check("Ts",Value(0.01)).asDouble()));

        T=options.check("T",Value(2.0)).asDouble();
        Kp=options.check("Kp",Value(10.0)).asDouble();
        Ki=options.check("Ki",Value(250.0)).asDouble();
        Kd=options.check("Kd",Value(15.0)).asDouble();
        vel_thres=fabs(options.check("vel_thres",Value(5.0)).asDouble());
        e_thres=fabs(options.check("e_thres",Value(1.0)).asDouble());

        gamma.resize(2,1.0);
        if (Bottle *pB=options.find("gamma").asList()) 
        {
            size_t len=std::min(gamma.length(),(size_t)pB->size());
            for (size_t i=0; i<len; i++)
                gamma[i]=pB->get(i).asDouble();
        }

        stiction.resize(2,0.0);
        if (Bottle *pB=options.find("stiction").asList()) 
        {
            size_t len=std::min(stiction.length(),(size_t)pB->size());
            for (size_t i=0; i<len; i++)
                stiction[i]=pB->get(i).asDouble();
        }

        return configured=true;
    }
    else
        return false;
}

// helper method for "yarpdev" body
static void toDox(PolyDriver& dd, FILE *os) {
    fprintf(os,"===============================================================\n");
    fprintf(os,"== Options checked by device:\n== \n");

    Bottle order = dd.getOptions();
    for (size_t i=0; i<order.size(); i++) {
        std::string name = order.get(i).toString();
        if (name=="wrapped"||(name.find(".wrapped")!=std::string::npos)) {
            continue;
        }
        std::string desc = dd.getComment(name.c_str());
        Value def = dd.getDefaultValue(name.c_str());
        Value actual = dd.getValue(name.c_str());
        std::string out = "";
        out += name;
        if (!actual.isNull()) {
            if (actual.toString()!="") {
                out += "=";
                if (actual.toString().length()<40) {
                    out += actual.toString().c_str();
                } else {
                    out += "(value too long)";
                }
            }
        }
        if (!def.isNull()) {
            if (def.toString()!="") {
                out += " [";
                if (def.toString().length()<40) {
                    out += def.toString().c_str();
                } else {
                    out += "(value too long)";
                }
                out += "]";
            }
        }
        if (desc!="") {
            out += "\n    ";
            out += desc.c_str();
        }
        fprintf(os,"%s\n", out.c_str());
    }
    fprintf(os,"==\n");
    fprintf(os,"===============================================================\n");
}

	virtual void threadRelease() {

		/* Stop and close ports */
		cportL->interrupt();
		cportR->interrupt();
		susPort->interrupt();

		cportL->close();
		cportR->close();
		susPort->close();

		delete cportL;
		delete cportR;
		delete susPort;

		/* stop motor interfaces and close */
		gaze->stopControl();
		clientGazeCtrl.close();
		carm->stopControl();
		clientArmCart.close();

		robotTorso.close();
		robotHead.close();
		robotArm.close();

	}

    bool close()
    {
        if (driver.isValid())
            driver.close();

        return true;
    }

    bool threadInit()
    {
        string name=rf.find("name").asString().c_str();
        double period=rf.find("period").asDouble();
        setRate(int(1000.0*period));

        // open a client interface to connect to the gaze server
        // we suppose that:
        // 1 - the iCub simulator (icubSim) is running
        // 2 - the gaze server iKinGazeCtrl is running and
        //     launched with the following options:
        //     --robot icubSim --context cameraCalibration/conf --config icubSimEyes.ini
        Property optGaze("(device gazecontrollerclient)");
        optGaze.put("remote","/iKinGazeCtrl");
        optGaze.put("local",("/"+name+"/gaze").c_str());

        if (!clientGaze.open(optGaze))
            return false;

        // open the view
        clientGaze.view(igaze);

        // latch the controller context in order to preserve
        // it after closing the module
        // the context contains the tracking mode, the neck limits and so on.
        igaze->storeContext(&startup_context_id);

        // set trajectory time:
        igaze->setNeckTrajTime(0.8);
        igaze->setEyesTrajTime(0.4);

        port.open(("/"+name+"/target:i").c_str());

        return true;
    }

    bool open(Searchable& p) {
        bool dev = true;
        if (p.check("nodevice")) {
            dev = false;
        }
        if (dev) {
            poly.open(p);
            if (!poly.isValid()) {
                printf("cannot open driver\n");
                return false;
            }
            
            if (!p.check("mute")) {
                // Make sure we can write sound
                poly.view(put);
                if (put==NULL) {
                    printf("cannot open interface\n");
                    return false;
                }
            }
        }
            
        port.setStrict(true);
        if (!port.open(p.check("name",Value("/yarphear")).asString())) {
            printf("Communication problem\n");
            return false;
        }
        
        if (p.check("remote")) {
            Network::connect(p.check("remote",Value("/remote")).asString(),
                             port.getName());
        }

        return true;
    }

    bool threadInit()
    {
        //initialize here variables
        printf("ControlThread:starting\n");
        
        Property options;
        options.put("device", "remote_controlboard");
        options.put("local", "/local/head");
        
        //substitute icubSim with icub for use with the real robot
        options.put("remote", "/icubSim/head");
        
        dd.open(options);

        dd.view(iencs);
        dd.view(ivel);

        if ( (!iencs) || (!ivel) )
            return false;
        
        int joints;
   
        iencs->getAxes(&joints);
    
        encoders.resize(joints);
        commands.resize(joints);

        commands=10000;
        ivel->setRefAccelerations(commands.data());

        count=0;
        return true;
    }

    virtual bool threadInit()
    {
        string name=rf.check("name",Value("faceTracker")).asString().c_str();
        string robot=rf.check("robot",Value("icub")).asString().c_str();
        int period=rf.check("period",Value(50)).asInt();
        eye=rf.check("eye",Value("left")).asString().c_str();
        arm=rf.check("arm",Value("right")).asString().c_str();
        eyeDist=rf.check("eyeDist",Value(1.0)).asDouble();
        holdoff=rf.check("holdoff",Value(3.0)).asDouble();

        Property optGaze("(device gazecontrollerclient)");
        optGaze.put("remote","/iKinGazeCtrl");
        optGaze.put("local",("/"+name+"/gaze_client").c_str());

        if (!clientGaze.open(optGaze))
            return false;

        clientGaze.view(igaze);
        igaze->storeContext(&startup_gazeContext_id);
        igaze->blockNeckRoll(0.0);

        if (arm!="none")
        {
            Property optArm("(device cartesiancontrollerclient)");
            optArm.put("remote",("/"+robot+"/cartesianController/"+arm+"_arm").c_str());
            optArm.put("local",("/"+name+"/arm_client").c_str());
    
            if (!clientArm.open(optArm))
                return false;
    
            clientArm.view(iarm);
            iarm->storeContext(&startup_armContext_id);
        }

        portImgIn.open(("/"+name+"/img:i").c_str());
        portImgOut.open(("/"+name+"/img:o").c_str());
        portTopDown.open(("/"+name+"/topdown:i").c_str());
        portSetFace.open(("/"+name+"/setFace:rpc").c_str());

        if (!fd.init(rf.findFile("descriptor").c_str()))
        {
            fprintf(stdout,"Cannot load descriptor!\n");
            return false;
        }

        Rand::init();

        resetState();
        armCmdState=0;
        queuedFaceExprFlag=false;

        setRate(period);
        cvSetNumThreads(1);

        t0=Time::now();

        return true;
    }