Exemplo n.º 1
0
int main(int argc, char *argv[])
{
    Network yarp;
    int c=0;

    BufferedPort<ImageOf<PixelRgb> > right;
    BufferedPort<ImageOf<PixelRgb> > left;

    BufferedPort<ImageOf<PixelRgb> > out;

    right.open("/teststereomatch/right");
    left.open("/teststereomatch/left");

    out.open("/teststereomatch/o");

    while(true)
    {
        ImageOf< PixelRgb> *imgR=right.read(true);
        ImageOf< PixelRgb> *imgL=left.read(true);

        ImageOf< PixelRgb> &outImg=out.prepare();

        if (imgR!=0 && imgL!=0)
                {
                    merge(*imgR, *imgL, outImg);

                    out.write();
                    c++;
                }
        printFrame(outImg.height(), outImg.width(), c);
    }
    return 0;
}
Exemplo n.º 2
0
 virtual void testStrictWriter() {
     report(0,"check strict writer...");
     BufferedPort<Bottle> in;
     BufferedPort<Bottle> out;
     in.setStrict();
     in.open("/in");
     out.open("/out");
     
     Network::connect("/out","/in");
     
     Bottle& outBot1 = out.prepare();
     outBot1.fromString("hello world");
     printf("Writing bottle 1: %s\n", outBot1.toString().c_str());
     out.write(true);
     
     Bottle& outBot2 = out.prepare();
     outBot2.fromString("2 3 5 7 11");
     printf("Writing bottle 2: %s\n", outBot2.toString().c_str());
     out.write(true);
     
     Bottle *inBot1 = in.read();
     checkTrue(inBot1!=NULL,"got 1 of 2 items");
     if (inBot1!=NULL) {
         printf("Bottle 1 is: %s\n", inBot1->toString().c_str());
         checkEqual(inBot1->size(),2,"match for item 1");
     }
     Bottle *inBot2 = in.read();
     checkTrue(inBot2!=NULL,"got 2 of 2 items");
     if (inBot2!=NULL) {
         printf("Bottle 2 is: %s\n", inBot2->toString().c_str());
         checkEqual(inBot2->size(),5,"match for item 1");
     }
 }
Exemplo n.º 3
0
    bool updateModule()
    {
        // get fresh images
        ImageOf<PixelRgb> *imgL=imgLPortIn.read();
        ImageOf<PixelRgb> *imgR=imgRPortIn.read();

        // interrupt sequence detected
        if ((imgL==NULL) || (imgR==NULL))
            return false;

        // compute the center-of-mass of pixels of our color
        mutex.lock();
        okL=getCOG(*imgL,cogL);
        okR=getCOG(*imgR,cogR);
        mutex.unlock();

        PixelRgb color;
        color.r=255; color.g=0; color.b=0;

        if (okL)
            draw::addCircle(*imgL,color,(int)cogL[0],(int)cogL[1],5);

        if (okR)
            draw::addCircle(*imgR,color,(int)cogR[0],(int)cogR[1],5);

        imgLPortOut.write(*imgL);
        imgRPortOut.write(*imgR);

        return true; 
    }
Exemplo n.º 4
0
	virtual void run()
	{

		// get inputs
		ImageOf<PixelFloat> *pSegIn=portImgIn->read(false);

		//if there is a seg image, use that
		if (pSegIn)
		{

			ImageOf<PixelRgb> *pImgIn=portRefIn->read(false);
			ImageOf<PixelRgb> &imgOut= portImgOut->prepare();

			Mat * M = new Mat(pSegIn->height(), pSegIn->width(), CV_32F, (void *)pSegIn->getRawImage());
			Mat X;
			vector<vector<Point> > contours;
			vector<Vec4i> hierarchy;

			M->convertTo(X,CV_8UC1);
			findContours(X, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

			//pick a random segmented object
			int iBlob = -1;
			if (contours.size() > 0) {
				iBlob = rand() % contours.size();
			}

			if (pImgIn) {
				imgOut.copy(*pImgIn);
			} else {
				imgOut.copy(*pSegIn);
			}

			//write the contour pixels of the object to port
			Matrix &object = portObjOut->prepare();
			yarp::sig::Vector xs, ys;
			vector<Point> cts = contours[iBlob];
			for (int i = 0; i < cts.size(); i++) {
				xs.push_back(cts.at(i).y);
				ys.push_back(cts.at(i).x);
				imgOut.pixel(cts.at(i).x,cts.at(i).y) = PixelRgb(255, 0, 0);
			}
			object.resize(xs.size(),2);
			object.setCol(0,xs);
			object.setCol(1,ys);

			portObjOut->write();
			portImgOut->write();

			delete M;

		}

	}
Exemplo n.º 5
0
int main() {
    // Initialize YARP - some OSes need network and time service initialization
    Network yarp;

    // Work locally - don't rely on name server (just for this example).
    // If you have a YARP name server running, you can remove this line.
    Network::setLocalMode(true);

    // Create two ports that we'll be using to transmit "Bottle" objects.
    // The ports are buffered, so that sending and receiving can happen
    // in the background.
    BufferedPort<Bottle> in;
    BufferedPort<Bottle> out;

    // we will want to read every message, with no skipping of "old" messages
    // when new ones come in
    in.setStrict();

    // Name the ports
    in.open("/in");
    out.open("/out");

    // Connect the ports so that anything written from /out arrives to /in
    Network::connect("/out","/in");

    // Send one "Bottle" object.  The port is responsible for creating
    // and reusing/destroying that object, since it needs to be sure
    // it exists until communication to all recipients (just one in
    // this case) is complete.
    Bottle& outBot1 = out.prepare();   // Get the object
    outBot1.fromString("hello world"); // Set it up the way we want
    printf("Writing bottle 1 (%s)\n", outBot1.toString().c_str());
    out.write();                       // Now send it on its way

    // Send another "Bottle" object
    Bottle& outBot2 = out.prepare();
    outBot2.fromString("2 3 5 7 11");
    printf("Writing bottle 2 (%s)\n", outBot2.toString().c_str());
    out.writeStrict();                 // writeStrict() will wait for any
                                       // previous communication to finish;
                                       // write() would skip sending if
                                       // there was something being sent

    // Read the first object
    Bottle *inBot1 = in.read();
    printf("Bottle 1 is: %s\n", inBot1->toString().c_str());

    // Read the second object
    Bottle *inBot2 = in.read();
    printf("Bottle 2 is: %s\n", inBot2->toString().c_str());

    return 0;
}
Exemplo n.º 6
0
int Repo::mygetch() {
    // can substitute next block for a timer:
    Time::delay(0.1); // seconds
    ///////////// [begin] The getch() implementation ///////////
    
    struct termios oldt, newt;
    int ch;
    tcgetattr( STDIN_FILENO, &oldt );
    newt = oldt;
    newt.c_lflag &= ~( ICANON | ECHO );
    tcsetattr( STDIN_FILENO, TCSANOW, &newt );
    ch = getchar();
    tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
    
    ///////////// [end] The getch() implementation ///////////

    Vector* qRead = port_q.read(false);
    if (qRead) {
        q = *qRead;  // joint position values
    } else {
        printf("Get joint values failed!\n");
        return -1;
    }

    Vector* f_eeRead = port_f.read(false);
    if (f_eeRead) {
        f_ee = *f_eeRead;  // EE force in end effector frame
    } else {
        printf("Get external contacts failed!\n");
        return -1;
    }
        
    d = q.subVector(0,4);
    for (int k=0;k<6;k++)
        d.push_back(f_ee[k]);
    printf("Sample %d: ",counter+1);  // Get on screen

    for (int k=0;k<11;k++) {
//        fprintf(stderr,"%f ",d[k]);  // Get on screen through stderr
        printf("%f ",d[k]);  // Sent to stdout to pass to file using operator >
        logger << d[k] << " ";  // Sent to logger file
    }
//    fprintf(stderr,"\n");  // Get on screen through stderr
    logger << std::endl;  // Sent to logger file

    printf("now I grabbed data\n");
    counter++;
//    return ch;
    return 0;
}
Exemplo n.º 7
0
    virtual void testAcquire() {
        report(0, "checking acquire/release...");

        BufferedPort<Bottle> in;
        BufferedPort<Bottle> out;
        in.setStrict();
        out.setStrict();
        in.open("/in");
        out.open("/out");
        Network::connect("/out","/in");

        out.prepare().fromString("1");
        out.write(true);
        
        Bottle *bot = in.read();
        checkTrue(bot!=NULL,"Inserted message received");
        if (bot!=NULL) {
            checkEqual(bot->size(),1,"right length");
        }

        out.prepare().fromString("1 2");
        out.write(true);

        void *key = in.acquire();
        Bottle *bot2 = in.read();
        checkTrue(bot2!=NULL,"Inserted message received");
        if (bot2!=NULL) {
            checkEqual(bot2->size(),2,"right length");
        }

        out.prepare().fromString("1 2 3");
        out.write(true);
        
        void *key2 = in.acquire();
        Bottle *bot3 = in.read();
        checkTrue(bot3!=NULL,"Inserted message received");
        if (bot3!=NULL) {
            checkEqual(bot3->size(),3,"right length");
        }
        if (bot2!=NULL) {
            checkEqual(bot2->size(),2,"original (2) still ok");
        }
        if (bot!=NULL) {
            checkEqual(bot->size(),1,"original (1) still ok");
        }

        in.release(key);
        in.release(key2);
    }
Exemplo n.º 8
0
    bool getBox()
    {
        // Crops the image based on user input and creates a template for the tracker with it.
        printf("Reading image!!\n");
        ImageOf<PixelRgb> *imgIn = imInPort.read();  // read an image
        cv::Mat img((IplImage*) imgIn->getIplImage());	   
     
        printf("Click first top left and then bottom right from the object !!\n");
        bool boxOK = false;
        //Bottle &out  = coordsOutPort.prepare();
        cv::Point tl, br;

        while (!boxOK){
            printf("Click on top left!\n");
            Bottle *point1 = coordsInPort.read(true);
            tl.x =  point1->get(0).asDouble();
            tl.y =  point1->get(1).asDouble();
            printf("Point read at %d, %d!!\n", tl.x, tl.y);

            printf("Click on bottom right!\n");
            Bottle *point2 = coordsInPort.read(true);            
            br.x =  point2->get(0).asDouble();
            br.y =  point2->get(1).asDouble();
            printf("Point read at %d, %d!!\n", br.x, br.y);

            BBox = cv::Rect(tl,br);            
            if (BBox.area() > 20) {
                printf("valid coordinates, cropping image!\n");
                boxOK = true;}
            else {printf("Coordinates not valid, click again!\n");}
        }

        printf("Prep out mat !!\n");
        ImageOf<PixelRgb> &templateOut  = tempOutPort.prepare();
        templateOut.resize(BBox.width, BBox.height);
        cv::Mat tempOut((IplImage*)templateOut.getIplImage(),false);
        img(BBox).copyTo(tempOut);
        //cv::GaussianBlur(img(BBox), imOut, cv::Size(1,1), 1, 1);

        double t0 = Time::now();
        while(Time::now()-t0 < 1) {  //send the template for one second
            printf("Writing Template!\n");
            tempOutPort.write();
            Time::delay(0.1);
        }

        tracking = true;
        return true;
    }
Exemplo n.º 9
0
    bool updateModule()
    {
        if (ImageOf<PixelRgb> *iImg=iPort.read())
        {
            LockGuard lg(mutex);
            ImageOf<PixelRgb> &oImg=oPort.prepare();
            oImg=*iImg;

            cv::Mat frame=cv::cvarrToMat((IplImage*)oImg.getIplImage());
            cv::Rect2d result;
            
            if (state==init)
            {
                tracker=cv::Tracker::create("BOOSTING");
                tracker->init(frame,initRect);
                result=initRect;
                state=run;
            }
            else if (state==run)
                tracker->update(frame,result);

            if (state!=idle)
                cv::rectangle(frame,
                              cv::Point((int)result.x,(int)result.y),
                              cv::Point((int)(result.x+result.width),(int)(result.y+result.height)),
                              cv::Scalar(0,255,0),2);

            oPort.write();
        }

        return true;
    }
Exemplo n.º 10
0
    void testPublisherToBufferedPort() {
        report(0,"Publisher to BufferedPort test");

        Node n("/node");
        Publisher<Bottle> p("/very_interesting_topic");

        {
            Node n2("/node2");
            BufferedPort<Bottle> pin;
            pin.setReadOnly();
            pin.setStrict();
            pin.open("very_interesting_topic");

            waitForOutput(p,10);

            Bottle& b = p.prepare();
            b.clear();
            b.addInt(42);
            p.write();
            p.waitForWrite();

            Bottle *bin = pin.read();
            checkTrue(bin!=NULL,"message arrived");
            if (!bin) return;
            checkEqual(bin->get(0).asInt(),42,"message is correct");
        }
    }
Exemplo n.º 11
0
    bool updateModule() {
        if (firstRun) {
            init();
            firstRun = false;
        }

        // get a target object position from a YARP port
        Bottle *b = inPort.read();

        if (b != NULL) {
            Vector xd(3);
            bool f;

            xd[0] = b->get(0).asDouble();
            xd[1] = b->get(1).asDouble();
            xd[2] = b->get(2).asDouble();

            // apply systematic offset
            // due to uncalibrated kinematic
            xd = xd + dOffs;

            // safe thresholding
            xd[0] = xd[0] > -0.1 ? -0.1 : xd[0];

            // grasp (wait until it's done)
            action->grasp(xd, graspOrien, graspDisp);
            action->checkActionsDone(f, true);
            action->areFingersInPosition(f);

            // if fingers are not in position,
            // it's likely that we've just grasped
            // something, so lift it up!
            if (!f) {
                cout << "Wow, got something!" << endl;

                // lift the object (wait until it's done)
                action->pushAction(xd + dLift, graspOrien);
                action->checkActionsDone(f, true);
            }
            else
                cout << "Sorry :( ... nothing to grasp" << endl;

            // release the object or just open the
            // hand (wait until it's done)
            action->pushAction("open_hand");
            action->checkActionsDone(f, true);

            // wait until it's done, since
            // use two arms that shares the torso
            action->pushAction(home_x);
            action->checkActionsDone(f, true);

            // let the hand wave a bit around home position
            // the waving will be disabled before commencing
            // a new action
            action->enableArmWaving(home_x);
        }

        return true;
    }
Exemplo n.º 12
0
    bool acquirePoints()
    {
        cout<<"point_cloud is reading from port..."<<endl;
        Bottle *pointIn=portIn.read(true);

        Vector point;
        point.resize(3,0.0);

        if (pointIn!=NULL)
        {
            int bsize=pointIn->size();
            cout<<"size of bottle received:  "<<bsize<<endl;
            for(size_t i=0; i<bsize; i++)
            {
                Bottle *pointList=pointIn->get(i).asList();

                point[0]=pointList->get(0).asDouble();
                point[1]=pointList->get(1).asDouble();
                point[2]=pointList->get(2).asDouble();

                cout<<"point received: "<<point.toString().c_str()<<endl;

                points.push_back(point);

            }

            return true;
        }
        else
            return false;

    }
Exemplo n.º 13
0
// Function: mdlOutputs =======================================================
// Abstract:
//   In this function, you compute the outputs of your S-function
//   block.
static void mdlOutputs(SimStruct *S, int_T tid)
{
    BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
    
    Vector *v = toPort->read(false); // Read from the port.  Waits until data arrives.
    if (v!=NULL)
    {
        for (int i = 0; i < SIZE_READING_PORT; i++)
        {
            real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);
            int_T widthPort = ssGetOutputPortWidth(S,i);
            if (widthPort == 1)
            {
                for(int_T j=0; j<widthPort; j++)
                    if (i < (v->length())){
                        pY[j] = v->data()[i];
			//cout<<v->data()[i]<<" ";
		    }
                    else
                        pY[j] = 0;
            }
            else
                cout << "ERROR: something wrong with port dimensions \n";
        }
    }
}
Exemplo n.º 14
0
int main(int argc, char *argv[]) {
    // Open the network
    Network yarp;
    BufferedPort<Sound> p;
    p.open("/receiver");
    Network::connect("/sender", "/receiver");
    // Get an audio write device.
    Property conf;
    conf.put("device","portaudio");
    conf.put("samples", "4096");
    conf.put("write", "1");
    PolyDriver poly(conf);
    IAudioRender *put;
    // Check it can write the sound
    poly.view(put);
    if (put==NULL) {
        printf("cannot open interface\n");
        return 1;
    }
    //Receive and render the sound
    Sound *s;
    while (true)
      {
    s = p.read(false);
    if (s!=NULL)
        put->renderSound(*s);
      }
    return 0;
}
Exemplo n.º 15
0
        bool updateModule()
        {
            Bottle *inputBottleTactileLeft = gtw_tactileLeftInputPort.read();
            Bottle *inputBottleTactileRight = gtw_tactileRightInputPort.read();

            Bottle &outputBottleTactileLeft = gtw_tactileLeftOutputPort.prepare();
            Bottle &outputBottleTactileRight = gtw_tactileRightOutputPort.prepare();

            outputBottleTactileLeft = *inputBottleTactileLeft;
            outputBottleTactileRight = *inputBottleTactileRight;

            gtw_tactileLeftOutputPort.write();
            gtw_tactileRightOutputPort.write();

            return true;
        }
Exemplo n.º 16
0
    bool updateModule()
    {
        if (imgOutPort.getOutputCount()>0)
        {
            if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
            {
                Vector xa,oa;
                iarm->getPose(xa,oa);

                Matrix Ha=axis2dcm(oa);
                Ha.setSubcol(xa,0,3);

                Vector v(4,0.0); v[3]=1.0;
                Vector c=Ha*v;

                v=0.0; v[0]=0.05; v[3]=1.0;
                Vector x=Ha*v;

                v=0.0; v[1]=0.05; v[3]=1.0;
                Vector y=Ha*v;

                v=0.0; v[2]=0.05; v[3]=1.0;
                Vector z=Ha*v;

                v=solution; v.push_back(1.0);
                Vector t=Ha*v;

                Vector pc,px,py,pz,pt;
                int camSel=(eye=="left")?0:1;
                igaze->get2DPixel(camSel,c,pc);
                igaze->get2DPixel(camSel,x,px);
                igaze->get2DPixel(camSel,y,py);
                igaze->get2DPixel(camSel,z,pz);
                igaze->get2DPixel(camSel,t,pt);

                CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
                CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
                CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
                CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
                CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);

                cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
                cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);

                cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);

                tip.clear();
                tip.addInt(point_t.x);
                tip.addInt(point_t.y);

                imgOutPort.prepare()=*pImgBgrIn;
                imgOutPort.write();
            }
        }

        return true;
    }
Exemplo n.º 17
0
	virtual void run()
	{

		// get both input images

        yarp::sig::Vector *actuation=actuationIn->read(false);


		//if we have the actuator
		if (actuation != NULL)
        //if (speaker->NotDone()) // this should actually loop some fixed number of blocks based on the update size
		{

            // setup output variables 
            yarp::os::Bottle &areaFunction = areaOut->prepare();
            yarp::os::Bottle &acousticSignal = acousticOut->prepare();


            // this should run some number of times? maybe...
            {
                // run next step of control inputs
                cout << actuation->data() << std::endl;
                for(int k = 0; k<kArt_muscle_MAX; k++){
                    speaker->art[k] = (*actuation)[k];
                    //cout << (*actuation)[k] << std::endl; // debug
                }

                // iterate simulator
                speaker->IterateSim();

                // loop back to start if we hit the of the buffer
                speaker->LoopBack();
            }


            // resize acousticSignal and put in samples
            //acousticSignal.resize(1); // (samples, channels) # of samples should correspond to loop above
            //acousticSignal(0) = speaker->getLastSample();
            acousticSignal.addDouble(speaker->getLastSample());

            // load area function 
            double temp[89];
            speaker->getAreaFcn(temp);

            // and pass into output variable
            //areaFunction.resize(89);
            for(int k=0;  k<89; k++){
                //areaFunction(k) = temp[k];
                areaFunction.addDouble(temp[k]);
            }

			//send out, cleanup
			areaOut->write();
			acousticOut->write();


		} else {
           // speaker->Speak();
        }
	}
Exemplo n.º 18
0
    virtual void run()
    {
        if (Vector *qdNew=qdPort.read(false))
            qd=*qdNew;

        Vector v=Kp*(qd-q);

        for (int i=0; i<joints; i++)
            if (v[i]>maxVel[i])
                v[i]=maxVel[i];
            else if (v[i]<-maxVel[i])
                v[i]=-maxVel[i];

        q=I->integrate(v);

        Vector &qo=qoPort.prepare();
        Vector &vo=voPort.prepare();

        qo.resize(3+joints,0.0);
        vo.resize(3+joints,0.0);

        // deal with torso joints
        for (int i=3; i<qo.length(); i++)
        {
            qo[i]=q[i-3];
            vo[i]=v[i-3];
        }

        qoPort.write();
        voPort.write();
    }
Exemplo n.º 19
0
int main(int argc, char *argv[]) {
    Network yarp;
    BufferedPort<Bottle> inPort;

    ResourceFinder parameters;
    parameters.configure(argc, argv);

    int delay=0;
    string portname="/consumer";

    if (parameters.check("name"))
        portname=parameters.find("name").asString().c_str();

    if (parameters.check("delay"))
    {
        delay=parameters.find("delay").asInt();
        printf("Setting delay to %d[ms]\n", delay);
    }

    inPort.open(portname.c_str());

    while (true) {
        Bottle *message=inPort.read();
        if (message==0)
            continue;

        int counter=message->get(0).asInt();
        string msg=message->get(1).asString().c_str();

        printf("Received: %d %s\n", counter, msg.c_str());

        Time::delay(delay/1000.0);
    }
    return 0;
}
Exemplo n.º 20
0
int main(int argc, char *argv[]) {

    // Initialize network
    Network yarp;

    // Make a port for reading and writing images
    BufferedPort<ImageOf<PixelRgb> > port;

    // Get command line options
    Property options;
    options.fromCommand(argc,argv);

    // Set the name of the port (use "/worker" if there is no --name option)
    std::string portName = options.check("name",Value("/worker")).asString();
    port.open(portName);
  
    int ct = 0;
    while (true) {
        // read an image from the port
        ImageOf<PixelRgb> *img = port.read();
        if (img==NULL) continue;

        // add a blue circle
        PixelRgb blue(0,0,255);
        addCircle(*img,blue,ct,50,10);
        ct = (ct+5)%img->width();

        // output the image
        port.prepare() = *img;
        port.write();
    }
  
    return 0;
}
Exemplo n.º 21
0
    void onRead(Sound& sound) {
        int ct = port.getPendingReads();
        //printf("pending reads %d\n", ct);
        while (ct>padding) {
            ct = port.getPendingReads();
            printf("Dropping sound packet -- %d packet(s) behind\n", ct);
            port.read();
        }
        mutex.wait();
        /*
          if (muted) {
          for (int i=0; i<sound.getChannels(); i++) {
          for (int j=0; j<sound.getSamples(); j++) {
          sound.put(0,j,i);
          }
          }
          }
        */
        if (!muted) {
            if (put!=NULL) {
                put->renderSound(sound);
            }
        }
        if (saving) {
            saveFrame(sound);
        }

        mutex.post();
        Time::yield();
    }
Exemplo n.º 22
0
    bool acquirePoints(int &file_provided, ResourceFinder &rf)
    {
        if(file_provided==1)
        {
            if (!rf.check("pointCloudFile"))
            {
                yError()<<"point clouds file not provided!";
                return false;
            }

            pointCloudFileName=rf.find("pointCloudFile"). asString().c_str();

            ifstream pointCloudFile(pointCloudFileName.c_str());
            if (!pointCloudFile.is_open())
            {
                yError()<<"problem opening point clouds file!";
                pointCloudFile.close();
                return false;
            }

            if (!readPoints(pointCloudFile))
            {
                yError()<<"problem reading point clouds file!";
                pointCloudFile.close();
                return false;
            }

            pointCloudFile.close();

        }
        else
        {
            cout<<"point_cloud is reading from port..."<<endl;
            Bottle *pointIn=portInPoints.read(true);
              if (pointIn!=NULL)
              {
                  int bsize=pointIn->size();
                  cout<<"size of bottle received:  "<<bsize<<endl;
                  for(size_t i=0; i<bsize; i++)
                  {
                      Bottle *pointList=pointIn->get(i).asList();

                      point[0]=pointList->get(0).asDouble();
                      point[1]=pointList->get(1).asDouble();
                      point[2]=pointList->get(2).asDouble();

                      point_cloud.push_back(point);
                      cout<<"point received "<<point.toString().c_str()<<endl;

                       nPointsC++;
                  }
                  cout<<"points acquired " << nPointsC<<endl;
                  return true;
               }

            portInPoints.close();

             }
        }
Exemplo n.º 23
0
	void SamIter(void)
	{
		Bottle *b = bRead.read(false);

		//while (bRead.getPendingReads() > 0)
		//	b = bRead.read(false); // get in the input from the port, if you want it to wait use true, else use false

		if(b!=NULL)						 // check theres data
		{
			
			if(b->size()>0)
			{
				task = b->get(0).asString();
				int iStatus=atoi(b->get(1).asString().c_str());
				//check if message is related to charging
				if(task.compare("ChargingStatus")==0)
				{
					
					//relay switch: off mobile base and close modules
					if(iStatus==0)
					{
						yarp::os::Time::delay(10);
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 0);
						system("stop.bat");
						bCharging=true;
					}
					else if(iStatus==1 && bCharging==true)//relay switch: on mobile base and start modules
					{
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 1);
						yarp::os::Time::delay(2);
						system("start.bat");
						bCharging=false;
					}
				}
				else
				{
					iflag = iStatus;
					//start timer 
					if(iflag==1)
						time (&start);
				}
			
			}

			std::cout << "got a task " << b->toString().c_str() << std::endl;
		}

		//send phone ring sensor data only if sensor value is changed
		if(bPhoneRingChanged)
		{
			// send back a bottle with current voltage value
			Bottle& b3 = bPhone.prepare();	  // prepare the bottle/port
			b3.clear();
			b3.addInt( iPhoneRing ); // indicates robot voltage		
			bPhone.writeStrict();
			bPhoneRingChanged=false;
		}

	}
Exemplo n.º 24
0
    bool updateModule()
    {
        if (imageIn.getInputCount()>0)
        {
            cv::Mat orig = (IplImage *) imageIn.read(true)->getIplImage();
            ImageOf<PixelMono> &outImg = imageOut.prepare();

            cv::Mat blueOnly = blueFilter(orig);
            
            float sumx=0, sumy=0;
            float num_pixel = 0;
            for(int x=0; x<blueOnly.cols; x++) {
                for(int y=0; y<blueOnly.rows; y++) {
                    int val = blueOnly.at<uchar>(y,x);
                    if( val >= 50) {
                        sumx += x;
                        sumy += y;
                        num_pixel++;
                    }
                }
            }
            cv::Point p(sumx/num_pixel, sumy/num_pixel);

            //cout << cv::Mat(p) << endl;

            cv::Moments m = cv::moments(blueOnly, false);
            cv::Point p1(m.m10/m.m00, m.m01/m.m00);
            //cout << cv::Mat(p1) << endl;

            cv::circle(blueOnly, p, 5, cv::Scalar(0,0,0), -1);

            Bottle &target=targetPort.prepare();
            target.clear();
            

            if (p.x < 0 && p.y <0) 
            {
                target.addDouble(0);
                target.addDouble(0);
                target.addInt(0);
            }            
            else
            {
                target.addDouble(p.x);
                target.addDouble(p.y);
                target.addInt(1);
            }

            IplImage tmp = blueOnly;
            outImg.resize(tmp.width, tmp.height);
            cvCopyImage( &tmp, (IplImage *) outImg.getIplImage());
            imageOut.write();

            targetPort.write();
        }
        return true;
    }
    // reading modified from react-control reactCtrlThread::getCollisionPointsFromPort
    // writing adapted from iCub_Sim.cpp OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
    bool fillSkinContactFromAggregPort(BufferedPort<Bottle> &inPort, const double amplification, skinContactList &sCL)
    {
        SkinPart sp = SKIN_PART_UNKNOWN;
        //all in the link FoR
        Vector geocenter(3,0.0); //geocenter from skin / average activation locus from the pps
        Vector normal(3,0.0);
        Vector force(3,0.0);
        Vector moment(3,0.0); //will remain zero
        double normalized_activation = 0.0;
        std::vector<unsigned int> taxel_list;
        taxel_list.clear(); //we will be always passing empty list

        Bottle* collPointsMultiBottle = inPort.read(false);

        if(collPointsMultiBottle != NULL)
        {
            //printf("fillSkinContactFromAggregPort(): There were %d bottles on the port.\n",collPointsMultiBottle->size());
            for(int i=0; i< collPointsMultiBottle->size();i++)
            {
                sp = SKIN_PART_UNKNOWN;
                geocenter.zero(); normal.zero();  normalized_activation = 0.0;
                Bottle* collPointBottle = collPointsMultiBottle->get(i).asList();
                //printf("Bottle %d contains %s \n", i,collPointBottle->toString().c_str());
                sp =  (SkinPart)(collPointBottle->get(0).asInt());
                geocenter(0) = collPointBottle->get(1).asDouble();
                geocenter(1) = collPointBottle->get(2).asDouble();
                geocenter(2) = collPointBottle->get(3).asDouble();
                normal(0) = collPointBottle->get(4).asDouble();
                normal(1) = collPointBottle->get(5).asDouble();
                normal(2) =  collPointBottle->get(6).asDouble();
                normalized_activation = collPointBottle->get(13).asDouble();

                //hack - in current iCubGui version, in bvhnode.h, ForceArrow,
                //there is an atan that gives a wrong direction for force vectors of the form (0 0 1)   
                force(0)=0.000001; force(1)=0.000001;
                force(2) = -1.0*amplification*normalized_activation*normal(2);
                
                //see  iCubGui/src/objectsthread.h    ObjectsManager::manage(iCub::skinDynLib::skinContactList &forces)
                //printf("fillDynContactFromAggregPort: setting dynContact: Body part: %s Linknum: %d CoP: %s F: %s M: %s\n",
                //BodyPart_s[SkinPart_2_BodyPart[sp].body].c_str(),getLinkNum(sp),geoCenter.toString(3,3).c_str(),
                //                              (-1.0*normal).toString(3,3).c_str(),moment.toString(3,3).c_str());

                skinContact sc(SkinPart_2_BodyPart[sp].body,sp,getLinkNum(sp),geocenter,geocenter,taxel_list,
                               amplification*normalized_activation,normal,force,moment);
                // In skinManager/src/compensator.cpp, Compensator::getContacts():
                // set an estimate of the force that is with normal direction and intensity equal to the pressure
                // d.setForceModule(-0.05*activeTaxels*pressure*normal);
                sCL.push_back(sc);
            }
            return true;
        }
        else{
            //printf("fillDynContactFromAggregPort(): no tactile/pps vectors on the port.\n") ;
            return false;
        };   
    }
 int main() {
 Network yarp; // set up yarp
 BufferedPort<ImageOf<PixelRgb> > imagePort;  // make a port for reading images
 BufferedPort<Vector> targetPort;
 imagePort.open("/tutorial/image/in");  // give the port a name
 targetPort.open("/tutorial/target/out");
 Network::connect("/icubSim/cam/left","/tutorial/image/in");
 

 while (1) { // repeat forever
   ImageOf<PixelRgb> *image = imagePort.read();  // read an image
   ImageOf<PixelRgb> *image_cropped;
   if (image!=NULL) { // check we actually got something
      printf("We got an image of size %dx%d\n", image->width(), image->height());
      double xMean = 0;
      double yMean = 0;
      int ct = 0;
      for (int x=0; x<image->width(); x++) {
        for (int y=0; y<image->height(); y++) {
          PixelRgb& pixel = image->pixel(x,y);
          /* very simple test for blueishness */
          /* make sure blue level exceeds red and green by a factor of 2 */
          if (pixel.b>pixel.r*1.2+10 && pixel.b>pixel.g*1.2+10) {
           /* there's a blueish pixel at (x,y)! */
           /* let's find the average location of these pixels */
           xMean += x;
           yMean += y;
           ct++;
          }
        }
      }
      if (ct>0) {
        xMean /= ct;
        yMean /= ct;
      }
      if (ct>(image->width()/20)*(image->height()/20)) {
        printf("Best guess at blue target: %g %g\n", xMean, yMean);
        Vector& target = targetPort.prepare();
        target.resize(3);
        target[0] = xMean;
        target[1] = yMean;
        target[2] = 1;
        targetPort.write();
      } else {
        Vector& target = targetPort.prepare();
        target.resize(3);
        target[0] = 0;
        target[1] = 0;
        target[2] = 0;
        targetPort.write();
      }
   }
 }
 return 0;
}
Exemplo n.º 27
0
    /*
    * This is our main function. Will be called periodically every getPeriod() seconds.
    */
    bool updateModule()
    {
		Bottle *bot=vergencePort.read(false);

		if(bot!=NULL && bot->toString()=="vergence_accomplished")
			vergence_accomplished=true;
		else
			vergence_accomplished=false;

        return true;
    }
Exemplo n.º 28
0
    void testReopen() {
        report(0,"checking opening/closing/reopening ports...");

        BufferedPort<Bottle> port2;
        port2.open("/test2");

        BufferedPort<Bottle> port;
        port.open("/test");

        Network::connect("/test2", "/test");
        Network::sync("/test");
        Network::sync("/test2");
        port2.prepare().fromString("1 msg");
        port2.write();

        while (port.getPendingReads()<1) {
            Time::delay(0.1);
        }

        checkFalse(port.isClosed(),"port tagged as open");
        port.close();
        checkTrue(port.isClosed(),"port tagged as closed");
        port.open("/test");
        checkFalse(port.isClosed(),"port tagged as open");

        Bottle *bot = port.read(false);
        checkTrue(bot==NULL,"reader correctly reset");

        Network::connect("/test2", "/test");
        Network::sync("/test");
        Network::sync("/test2");
        port2.prepare().fromString("2 msg");
        port2.write();

        bot = port.read();
        checkFalse(bot==NULL,"reader working");
        if (bot) {
            checkEqual(bot->get(0).asInt(),2,"reader read correct message");
        }
    }
Exemplo n.º 29
0
    bool getPointTrack(double tableHeight)
    {
        // Put the input image at the moment of computing out
        printf("Propagating image!!\n");
        ImageOf<PixelRgb> *imgIn = imInPort.read();  // read an image
		if(imgIn == NULL)		{
            printf("No object tracked \n");
			return false;
		}	
        ImageOf<PixelRgb> &imOut  = imOutPort.prepare();
        imOut = *imgIn;
        imOutPort.write();
       
        // Retrieves and relays the 2D coordinates of the object tracked by the tracker
        //Bottle &out  = coordsOutPort.prepare();
        printf("Getting 3D coords of tracked object!!\n");
        Bottle *trackCoords = trackInPort.read(true);
        coords2D.resize(2,0);        
        coords2D[0] =  trackCoords->get(0).asInt();
        coords2D[1] =  trackCoords->get(1).asInt();
        printf("Point in 2D read: %.2f, %.2f!!\n", coords2D(0), coords2D(1));

        Vector table(4);  // specify the plane in the root reference frame as ax+by+cz+d=0; z=-tableHeight in this case
        table[0] = 0.0; table[1] = 0.0; table[2] = 1.0;  
        table[3] = -tableHeight;    // d -> so the equation of the table plane is z=-h 

        int camSel;
        if (camera != "left") {
               camSel = 1;}
        else { camSel = 0;}    

        if(igaze->get3DPointOnPlane(camSel,coords2D, table, coords3D)){
            printf("Point in 3D computed: %.2f, %.2f, %.2f!!\n", coords3D(0), coords3D(1), coords3D(2));
            return true;
	    } else  {
            printf("3D point could not be computed\n");
            return false;
        }
    }
Exemplo n.º 30
0
 bool getPointClick()
 {
     printf("Click on the object in the image!!\n");
     //Bottle &out  = coordsOutPort.prepare();
     Bottle *obj2Dcoord = coordsInPort.read(true);
     printf("Point read!!\n");
     coords2D.resize(2,0.0);        
     coords2D[0] =  obj2Dcoord->get(0).asInt();
     coords2D[1] =  obj2Dcoord->get(1).asInt();
     printf("Point in 2D read: %.2f, %.2f!!\n", coords2D(0), coords2D(1));
    
     return true;    // get the projection
 }