C++ PolyDriver::open方法代码示例

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;
}

    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;
    }

    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 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;
 }

    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;
    }

    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 configure(ResourceFinder &rf)
    {
        Property options;
        options.put("device","fakeyServer");
        options.put("local","/fake_robot/fake_part");

        driver.open(options);
        return driver.isValid();
    }

    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;
    }

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;
}

 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 configure(ResourceFinder &rf)
    {
        string mode=rf.check("mode",Value("physical")).asString().c_str();

        Property option;
        if (mode=="physical")
            option.put("device","geomagicdriver");
        else
        {
            option.put("device","hapticdeviceclient"); 
            option.put("remote","/hapticdevice");
            option.put("local","/client");
        }

        if (!driver.open(option))
            return false;

        driver.view(igeo);
        return true;
    }

/** Lorenzo Natale, Dec 2007. Test remotization of calibrate 
 *  functions. Useful for general tests on device remotization 
 *  (see fakebot for example).
 */
int main(int argc, const char **argv)
{
    Network yarp;
    IControlCalibration2 *ical;

    Property p;
    p.put("device", "remote_controlboard");
    p.put("local", "/motortest");     //prefix for local names
    p.put("remote", "/controlboard"); //prefix for remote names

    PolyDriver device;
    device.open(p);
    device.view(ical);

    //ical->calibrate();
    ical->calibrate2(1,1000,1.1,1.1,1.1);
    ical->done(2);
    ical->park();

    device.close();
    return 0; 
}

    /*
     * init the thread, instantiate the device driver and connet the ports appropriately.
     * @return true iff successful.
     */
    virtual bool threadInit() {
        Property p;
        p.put("device", "logpolarclient");
        p.put("local", local.c_str());
        p.put("remote", remote.c_str());
        poly.open(p);
        if (poly.isValid()) {
            poly.view(lpImage);
            active = false;

            if (lpImage != 0) {
                const int nang = lpImage->nang();
                const int necc = lpImage->necc();
                const int fovea = lpImage->fovea();
                const double overlap = lpImage->overlap();

                if (necc != 0 && nang != 0) {
                    fprintf(stdout, "logpolar format with ang: %d ecc: %d fov: %d ovl: %f\n",
                        nang,
                        necc,
                        fovea,
                        overlap);

                    fprintf(stdout, "cartesian image of size %d %d\n", width, height);
                    trsf.allocLookupTables(L2C, necc, nang, width, height, overlap);
                    active = true;

                    lp.resize(nang, necc);
                    
                    // open the out port.
                    out.open(outname.c_str());
                }
            }
        }
        return active;
    }

    bool configure(ResourceFinder &rf)
    {
        bool noLegs=rf.check("no_legs");

        Property optTorso("(device remote_controlboard)");
        Property optArmR("(device remote_controlboard)");
        Property optArmL("(device remote_controlboard)");
        Property optLegR("(device remote_controlboard)");
        Property optLegL("(device remote_controlboard)");

        string robot=rf.find("robot").asString().c_str();
        optTorso.put("remote",("/"+robot+"/torso").c_str());
        optArmR.put("remote",("/"+robot+"/right_arm").c_str());
        optArmL.put("remote",("/"+robot+"/left_arm").c_str());
        optLegR.put("remote",("/"+robot+"/right_leg").c_str());
        optLegL.put("remote",("/"+robot+"/left_leg").c_str());

        string local=rf.find("local").asString().c_str();
        optTorso.put("local",("/"+local+"/torso").c_str());
        optArmR.put("local",("/"+local+"/right_arm").c_str());
        optArmL.put("local",("/"+local+"/left_arm").c_str());
        optLegR.put("local",("/"+local+"/right_leg").c_str());
        optLegL.put("local",("/"+local+"/left_leg").c_str());

        optTorso.put("writeStrict","on");
        optArmR.put("writeStrict","on");
        optArmL.put("writeStrict","on");
        optLegR.put("writeStrict","on");
        optLegL.put("writeStrict","on");

        if (!torso.open(optTorso) || !armR.open(optArmR) || !armL.open(optArmL))
        {
            cout<<"Device drivers not available!"<<endl;
            close();

            return false;
        }

        if (!noLegs)
        {
            if (!legR.open(optLegR) || !legL.open(optLegL))
            {
                cout<<"Device drivers not available!"<<endl;
                close();

                return false;
            }
        }

        PolyDriverList listArmR, listArmL, listLegR, listLegL;
        listArmR.push(&torso,"torso");
        listArmR.push(&armR,"right_arm");
        listArmL.push(&torso,"torso");
        listArmL.push(&armL,"left_arm");
        listLegR.push(&legR,"right_leg");
        listLegL.push(&legL,"left_leg");

        Property optServerArmR("(device cartesiancontrollerserver)");
        Property optServerArmL("(device cartesiancontrollerserver)");
        Property optServerLegR("(device cartesiancontrollerserver)");
        Property optServerLegL("(device cartesiancontrollerserver)");
        optServerArmR.fromConfigFile(rf.findFile("right_arm_file"),false);
        optServerArmL.fromConfigFile(rf.findFile("left_arm_file"),false);
        optServerLegR.fromConfigFile(rf.findFile("right_leg_file"),false);
        optServerLegL.fromConfigFile(rf.findFile("left_leg_file"),false);

        if (!serverArmR.open(optServerArmR) || !serverArmL.open(optServerArmL))
        {
            close();    
            return false;
        }

        if (!noLegs)
        {
            if (!serverLegR.open(optServerLegR) || !serverLegL.open(optServerLegL))
            {
                close();    
                return false;
            }
        }

        IMultipleWrapper *wrapperArmR, *wrapperArmL, *wrapperLegR, *wrapperLegL;
        serverArmR.view(wrapperArmR);
        serverArmL.view(wrapperArmL);
        serverLegR.view(wrapperLegR);
        serverLegL.view(wrapperLegL);

        if (!wrapperArmR->attachAll(listArmR) || !wrapperArmL->attachAll(listArmL))
        {
            close();    
            return false;
        }

        if (!noLegs)
        {
            if (!wrapperLegR->attachAll(listLegR) || !wrapperLegL->attachAll(listLegL))
            {
                close();    
                return false;
            }
//.........这里部分代码省略.........

    virtual bool threadInit()
    {
        // open a client interface to connect to the cartesian server of the simulator
        // we suppose that:
        //
        // 1 - the iCub simulator is running
        //     (launch: iCub_SIM)
        //
        // 2 - the cartesian server is running
        //     (launch: yarprobotinterface --context simCartesianControl)
        //
        // 3 - the cartesian solver for the left arm is running too
        //     (launch: iKinCartesianSolver --context simCartesianControl --part left_arm)
        //
        Property option("(device cartesiancontrollerclient)");
        option.put("remote","/icubSim/cartesianController/left_arm");
        option.put("local","/cartesian_client/left_arm");

        if (!client.open(option))
            return false;

        // open the view
        client.view(icart);

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

        // set trajectory time
        icart->setTrajTime(1.0);

        // get the torso dofs
        Vector newDof, curDof;
        icart->getDOF(curDof);
        newDof=curDof;

        // enable the torso yaw and pitch
        // disable the torso roll
        newDof[0]=1;
        newDof[1]=0;
        newDof[2]=1;
        
        // send the request for dofs reconfiguration
        icart->setDOF(newDof,curDof);
        
        // impose some restriction on the torso pitch
        limitTorsoPitch();       

        // print out some info about the controller
        Bottle info;
        icart->getInfo(info);
        fprintf(stdout,"info = %s\n",info.toString().c_str());

        // register the event, attaching the callback
        icart->registerEvent(*this);

        xd.resize(3);
        od.resize(4);

        return true;
    }