Esempio n. 1
0
	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();

	}
Esempio n. 2
0
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;
}
Esempio n. 3
0
    bool close()
    {
        if (driver.isValid())
            driver.close();

        return true;
    }
Esempio n. 4
0
    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;
    }
Esempio n. 5
0
    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;
    }
Esempio n. 7
0
 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;
 }
Esempio n. 8
0
    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;
    }
Esempio n. 9
0
    bool configure(ResourceFinder &rf)
    {
        Property options;
        options.put("device","fakeyServer");
        options.put("local","/fake_robot/fake_part");

        driver.open(options);
        return driver.isValid();
    }
Esempio n. 10
0
    virtual bool threadInit()
    {
        // open a client interface to connect to the gaze server
        // we suppose that:
        // 1 - the iCub simulator is running;
        // 2 - the gaze server iKinGazeCtrl is running and
        //     launched with the following options: "--from configSim.ini"
        Property optGaze("(device gazecontrollerclient)");
        optGaze.put("remote","/iKinGazeCtrl");
        optGaze.put("local","/gaze_client");

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

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

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

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

        if (!clientTorso.open(optTorso))
            return false;

        // open the view
        clientTorso.view(ienc);
        clientTorso.view(ipos);
        ipos->setRefSpeed(0,10.0);

        fp.resize(3);

        state=STATE_TRACK;

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

        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;
    }
Esempio n. 12
0
    virtual void threadRelease()
    {    
        // we require an immediate stop
        // before closing the client for safety reason
        igaze->stopControl();

        // it's a good rule to restore the controller
        // context as it was before opening the module
        igaze->restoreContext(startup_context_id);

        clientGaze.close();
        clientTorso.close();
    }
Esempio n. 13
0
    bool close()
    {
        igaze->restoreContext(startup_context_gaze);
        drvGaze.close();

        iarm->restoreContext(startup_context_arm);
        drvArm.close();

        imgLPortIn.close();
        imgRPortIn.close();
        imgLPortOut.close();
        imgRPortOut.close();
        rpcPort.close();
        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;
    }
Esempio n. 15
0
 Gazer(PolyDriver &driver, ActionPrimitives *action, const Vector &offset) : 
       RateThread(50)
 {
     driver.view(this->igaze);
     this->action=action;
     this->offset=offset;
 }
Esempio n. 16
0
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;
}
Esempio n. 17
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;
    }
Esempio n. 18
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");
 }
Esempio n. 19
0
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;
}
Esempio n. 20
0
// 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");
}
Esempio n. 21
0
    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);
    }
Esempio n. 22
0
    bool close()
    {
        driver.close();

        Property options;
        model->toProperty(options);
        fprintf(stdout,"model options: %s\n",options.toString().c_str());
        
        return true;
    }
Esempio n. 23
0
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 threadRelease()
    {
        printf("ControlThread:stopping the robot\n");
        
        ivel->stop();

        dd.close();

        printf("Done, goodbye from ControlThread\n");
    }
    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;
    }
    //********************************************
    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;
    }
Esempio n. 27
0
/** 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; 
}
Esempio n. 28
0
    virtual bool close()
    {
        yInfo("Closing module [%s]\n", partName);
        vc->stop();
        vc->halt();
        delete vc;
        yInfo("Thead [%s] stopped\n", partName);

        driver.close();
        input_port.close();

        yInfo("Module [%s] closed\n", partName);
        return true;
    }
Esempio n. 29
0
    virtual bool close()
    {
        yInfo("Closing module [%s]\n", partName);
        pThread->stop();
        pThread->halt();
        delete pThread;
        yInfo("Thread [%s] stopped\n", partName);

        driver.close();
        rpc_port.close();

        yInfo("Module [%s] closed\n", partName);
        return true;
    }
Esempio n. 30
0
static void toDox(PolyDriver& dd, FILE *os) {
    fprintf(os, "<table>\n");
    fprintf(os, "<tr><td>PROPERTY</td><td>DESCRIPTION</td><td>DEFAULT</td></tr>\n");
    Bottle order = dd.getOptions();
    for (int i=0; i<order.size(); i++) {
        String name = order.get(i).toString().c_str();
        if (name=="wrapped"||name.substr(0,10)=="subdevice.") {
            continue;
        }
        ConstString desc = dd.getComment(name.c_str());
        ConstString def = dd.getDefaultValue(name.c_str()).toString();
        String out = "";
        out += "<tr><td>";
        out += name.c_str();
        out += "</td><td>";
        out += desc.c_str();
        out += "</td><td>";
        out += def.c_str();
        out += "</td></tr>";
        fprintf(os,"%s\n",out.c_str());
    }
    fprintf(os, "</table>\n");
}