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

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

    //********************************************
    bool close()
    {
        yInfo("[demoAvoidance] Closing module..");

        dataPort.close();

        if (useLeftArm)
        {
            if (driverCartL.isValid())
            {
                data["left"].iarm->stopControl();
                data["left"].iarm->restoreContext(contextL);
                driverCartL.close();
            }

            if (driverJointL.isValid())
            {
                IInteractionMode *imode;
                driverJointL.view(imode);
                for (int j=0; j<5; j++)
                    imode->setInteractionMode(j,VOCAB_IM_STIFF);

                driverJointL.close();
            }
        }

        if (useRightArm)
        {
            if (driverCartR.isValid())
            {
                data["right"].iarm->stopControl();
                data["right"].iarm->restoreContext(contextR);
                driverCartR.close();
            }

            if (driverJointR.isValid())
            {
                IInteractionMode *imode;
                driverJointR.view(imode);
                for (int j=0; j<5; j++)
                    imode->setInteractionMode(j,VOCAB_IM_STIFF);

                driverJointR.close();
            }
        }

        return true;
    }

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

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

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

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

void testMotor(PolyDriver& driver) {
    IPositionControl *pos;
    if (driver.view(pos)) {
        int ct = 0;
        pos->getAxes(&ct);
        printf("  number of axes is: %d\n", ct);
    } else {
        printf("  could not find IPositionControl interface\n");
    }
}

    void idlingCoupledJoints(const int i, const bool sw)
    {
        IControlMode2 *imod;
        driver->view(imod);

        PidData &pid=pids[i];
        for (size_t j=0; j<pid.idling_joints.size(); j++)
            imod->setControlMode(pid.idling_joints[j],
                                 sw?VOCAB_CM_IDLE:VOCAB_CM_POSITION);
    }

    //********************************************
    bool close()
    {
        dataPort.close();

        if (driverCartL.isValid())
        {
            data["left"].iarm->stopControl();
            data["left"].iarm->restoreContext(contextL);
            driverCartL.close(); 
        }

        if (driverCartR.isValid())
        {
            data["right"].iarm->stopControl();
            data["right"].iarm->restoreContext(contextR);
            driverCartR.close();
        }

        if (driverJointL.isValid())
        {
            IInteractionMode *imode;
            driverJointL.view(imode);
            for (int j=0; j<5; j++)
                imode->setInteractionMode(j,VOCAB_IM_STIFF);

            driverJointL.close();
        }

        if (driverJointR.isValid())
        {
            IInteractionMode *imode;
            driverJointR.view(imode);
            for (int j=0; j<5; j++)
                imode->setInteractionMode(j,VOCAB_IM_STIFF);

            driverJointR.close();
        }

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