Ejemplo n.º 1
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;
    }
Ejemplo n.º 2
0
    bool configure(ResourceFinder &rf)
    {
        string robot=rf.check("robot",Value("icub")).asString().c_str();
        string name=rf.check("name",Value("karmaToolFinder")).asString().c_str();
        arm=rf.check("arm",Value("right")).asString().c_str();
        eye=rf.check("eye",Value("left")).asString().c_str();

        if ((arm!="left") && (arm!="right"))
        {
            printf("Invalid arm requested!\n");
            return false;
        }

        if ((eye!="left") && (eye!="right"))
        {
            printf("Invalid eye requested!\n");
            return false;
        }

        Property optionArmL("(device cartesiancontrollerclient)");
        optionArmL.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
        optionArmL.put("local",("/"+name+"/left_arm").c_str());
        if (!drvArmL.open(optionArmL))
        {
            printf("Cartesian left_arm controller not available!\n");
            terminate();
            return false;
        }

        Property optionArmR("(device cartesiancontrollerclient)");
        optionArmR.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
        optionArmR.put("local",("/"+name+"/right_arm").c_str());
        if (!drvArmR.open(optionArmR))
        {
            printf("Cartesian right_arm controller not available!\n");
            terminate();
            return false;
        }

        if (arm=="left")
            drvArmL.view(iarm);
        else
            drvArmR.view(iarm);

        Property optionGaze("(device gazecontrollerclient)");
        optionGaze.put("remote","/iKinGazeCtrl");
        optionGaze.put("local",("/"+name+"/gaze").c_str());
        if (drvGaze.open(optionGaze))
            drvGaze.view(igaze);
        else
        {
            printf("Gaze controller not available!\n");
            terminate();
            return false;
        }

        Bottle info;
        igaze->getInfo(info);
        if (Bottle *pB=info.find(("camera_intrinsics_"+eye).c_str()).asList())
        {
            int cnt=0;
            Prj.resize(3,4);
            for (int r=0; r<Prj.rows(); r++)
                for (int c=0; c<Prj.cols(); c++)
                    Prj(r,c)=pB->get(cnt++).asDouble();
        }
        else
        {
            printf("Camera intrinsic parameters not available!\n");
            terminate();
            return false;
        }

        imgInPort.open(("/"+name+"/img:i").c_str());
        imgOutPort.open(("/"+name+"/img:o").c_str());
        dataInPort.open(("/"+name+"/in").c_str());
        logPort.open(("/"+name+"/log:o").c_str());
        rpcPort.open(("/"+name+"/rpc").c_str());
        attach(rpcPort);

        Vector min(3),max(3);
        min[0]=-1.0; max[0]=1.0;
        min[1]=-1.0; max[1]=1.0;
        min[2]=-1.0; max[2]=1.0;
        solver.setBounds(min,max);
        solution.resize(3,0.0);

        enabled=false;
        dataInPort.setReader(*this);

        return true;
    }