Esempio n. 1
0
 void checkCallback() {
     report(0, "checking callback...");
     BufferedPort<Bottle> out;
     PortReaderBufferTestHelper in;
     out.open("/out");
     in.open("/in");
     in.useCallback();
     Network::connect("/out","/in");
     Network::sync("/out");
     Network::sync("/in");
     out.prepare().fromString("1 2 3");
     out.write();
     int rep = 0;
     while (in.count==0 && rep<50) {
         Time::delay(0.1);
         rep++;
     }
     checkEqual(in.count,3,"got message #1");
     in.disableCallback();
     out.prepare().fromString("1 2 3 4");
     out.write(true);
     Bottle *datum = in.read();
     checkTrue(datum!=NULL, "got message #2");
     checkEqual(datum->size(),4,"message is ok");
     in.useCallback();
     in.count = 0;
     out.prepare().fromString("1 2 3 4 5");
     out.write(true);
     rep = 0;
     while (in.count==0 && rep<50) {
         Time::delay(0.1);
         rep++;
     }
     checkEqual(in.count,5,"got message #3");
 }
Esempio n. 2
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();
        }
	}
Esempio n. 3
0
    virtual void testRecentReader() {
        report(0,"check recent reader...");
        BufferedPort<Bottle> in;
        BufferedPort<Bottle> out;
        in.setStrict(false);
        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);

        Time::delay(0.25);

        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");
        }
    }
Esempio n. 4
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();
    }
Esempio n. 5
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;
    }
 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;
}
Esempio n. 7
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;

		}

	}
Esempio n. 8
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;
}
Esempio n. 9
0
    bool updateModule()
    {
        Vector *imuData=iPort.read();
        if (imuData==NULL)
            return false;

        Stamp stamp;
        iPort.getEnvelope(stamp);

        double t0=Time::now();
        Vector gyro=imuData->subVector(6,8);
        Vector gyro_filt=gyroFilt.filt(gyro);

        gyro-=gyroBias;
        gyro_filt-=gyroBias;

        Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
        double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));

        adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
        gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
        double dt=Time::now()-t0;

        if (oPort.getOutputCount()>0)
        {
            Vector &outData=oPort.prepare();
            outData=*imuData;
            outData.setSubvector(6,gyro);
            oPort.setEnvelope(stamp);
            oPort.write();
        }

        if (bPort.getOutputCount()>0)
        {
            bPort.prepare()=gyroBias;
            bPort.setEnvelope(stamp);
            bPort.write();
        }

        if (verbose)
        {
            yInfo("magVel   = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
            yInfo("gyro     = %s",gyro.toString(3,3).c_str());
            yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
            yInfo("dt       = %.0f [us]",dt*1e6);
            yInfo("\n");
        }

        return true;
    }
Esempio n. 10
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);
    }
Esempio n. 11
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;
}
Esempio n. 12
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;
        }
Esempio n. 13
0
    virtual void onRead(TestData &datum)
    {
        if (count==nframes)
            return;


        double now=Time::now();
        double origT=datum.get();
        unsigned int pl=datum.getPayloadSize();


#ifdef USE_PARALLEL_PORT
        static ppEventDebugger pp(0x378);
        pp.set();
        pp.reset();
#endif

        TestData &nd=outPort.prepare();
        nd.set(origT);
        nd.resize(pl);
        outPort.write(true);

        if (wait<=0)
            {

                double dT=(now-origT)*1000;
                delay+=dT;
                delaySq+=(dT*dT);
                count++;

                latencies.push_back(Report(dT, pl));
            }
        else
            wait--;
    }
Esempio n. 14
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;
}
Esempio n. 15
0
    void testUnbufferedSubscriber() {
        report(0,"Unbuffereded Subscriber test");

        Node n("/node");
        BufferedPort<Bottle> pout;
        pout.setWriteOnly();
        pout.open("very_interesting_topic");

        {
            Node n2("/node2");
            Subscriber<Bottle> pin("/very_interesting_topic");

            waitForOutput(pout,10);

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

            Bottle bin;
            bin.addInt(99);
            pin.read(bin);
            pout.waitForWrite();
            checkEqual(bin.get(0).asInt(),42,"message is correct");
        }
    }
Esempio n. 16
0
 bool updateModule()
 {
     ImageOf<PixelRgb> &img = portOut.prepare();
     global->readAndShow(img, showImages);
     portOut.write();
     return true;
 }
Esempio n. 17
0
	void SamIter(void)
	{
	bool thereIsObservation,hardError;	
	CObservation2DRangeScan myscan;
	
	
    laser.doProcessSimple( thereIsObservation, myscan, hardError );
	Bottle &Send = myfirst.prepare();
	Send.clear();

	if(thereIsObservation )
		{
			//scan only 512 points for 180 deg, ignore the first,last 85 points
			for(int x=0;x<myscan.scan.size()-85;x++)
			{
				//if(myscan.scan[x]<mindistance){myscan.scan[x]=0;}
				if(x>84) 
				{
					Send.addDouble(myscan.scan[x]);

					if(myscan.scan[x]==1.0)
						std::cout << "obst sensed at " << x <<std::endl;
					
					//if(myscan.scan[x]<mindistance){myscan.scan[x]=0;}
				}
				//std::cout << "Scan size Vlaser" << myscan.scan.size() <<std::endl;
			}
		myfirst.writeStrict();
		}
	//std::cout<< " laser size " <<Send.size() <<std::endl;
	}
Esempio n. 18
0
    bool threadInit()
    {
        fprintf(stdout,"init \n");
        port_in0.open(string("/simCOM0:i").c_str());
        port_in1.open(string("/simCOM1:i").c_str());
        port_in2.open(string("/simCOM2:i").c_str());
        port_in3.open(string("/simCOM3:i").c_str());
        port_in4.open(string("/simCOM4:i").c_str());
        port_in5.open(string("/simCOM5:i").c_str());
        port_in6.open(string("/simCOM6:i").c_str());
        port_in7.open(string("/simCOM7:i").c_str());
        port_in8.open(string("/simCOM8:i").c_str());
        port_out.open(string("/simCOM:o").c_str());
        yarp::os::Network::connect("/wholeBodyDynamics/com:o","/simCOM0:i");
        yarp::os::Network::connect("/wholeBodyDynamics/left_leg/com:o","/simCOM1:i");
        yarp::os::Network::connect("/wholeBodyDynamics/left_arm/com:o","/simCOM2:i");
        yarp::os::Network::connect("/wholeBodyDynamics/right_leg/com:o","/simCOM3:i");
        yarp::os::Network::connect("/wholeBodyDynamics/right_arm/com:o","/simCOM4:i");
        yarp::os::Network::connect("/wholeBodyDynamics/head/com:o","/simCOM5:i");
        yarp::os::Network::connect("/wholeBodyDynamics/torso/com:o","/simCOM6:i");
        yarp::os::Network::connect("/wholeBodyDynamics/upper_body/com:o","/simCOM7:i");
        yarp::os::Network::connect("/wholeBodyDynamics/lower_body/com:o","/simCOM8:i");
        yarp::os::Network::connect("/simCOM:o","/iCubGui/objects");

        Bottle a;
        a.clear();
        a.addString("reset");
        port_out.prepare() = a;
        port_out.write();

        return true;
    }
Esempio n. 19
0
    void testBufferedPortToSubscriber() {
        report(0,"BufferedPort to Subscriber test");

        Node n("/node");
        BufferedPort<Bottle> pout;
        pout.setWriteOnly();
        pout.open("very_interesting_topic");

        {
            Node n2("/node2");
            Subscriber<Bottle> pin("/very_interesting_topic");
            pin.read(false); // make sure we are in buffered mode

            waitForOutput(pout,10);

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

            Bottle *bin = pin.read();
            checkTrue(bin!=NULL,"message arrived");
            if (!bin) return;
            checkEqual(bin->get(0).asInt(),42,"message is correct");
        }
    }
Esempio n. 20
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;
    }
Esempio n. 21
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;
    }
Esempio n. 22
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;
		}

	}
Esempio n. 23
0
    bool updateModule()
    {
        LockGuard lg(mutex);

        double dumpTime=Time::now();
        Bottle &bDump=dumpPort.prepare();
        bDump.clear();

        bDump.addString(actionTag);
        bDump.addString(objectTag);

        opc.checkout();        

        // agent body + position
        agentName = getPartnerName();
        if (Entity *e=opc.getEntity(agentName))
        {
            if (Agent *agent=dynamic_cast<Agent*>(e))
            {
                bDump.addList()=agent->m_body.asBottle();
                Bottle &bAgent=bDump.addList();
                bAgent.addString(agent->name());
                bAgent.addDouble(agent->m_ego_position[0]);
                bAgent.addDouble(agent->m_ego_position[1]);
                bAgent.addDouble(agent->m_ego_position[2]);
                bAgent.addInt(agent->m_present==1.0?1:0);
            }
        }

        // objects position
        list<Entity*> lEntity=opc.EntitiesCache();
        for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
        {
            string entityName=(*itEnt)->name();
            string entityType=(*itEnt)->entity_type();
            if (entityType==EFAA_OPC_ENTITY_OBJECT)
            {
                if (Object *object=dynamic_cast<Object*>(*itEnt))
                {
                    Bottle &bObject=bDump.addList();
                    bObject.addString(object->name());
                    bObject.addDouble(object->m_ego_position[0]);
                    bObject.addDouble(object->m_ego_position[1]);
                    bObject.addDouble(object->m_ego_position[2]);
                    bObject.addInt(object->m_present==1.0?1:0);
                }
            }
        }

        bDump.addInt(gate);        

        dumpStamp.update(dumpTime);
        dumpPort.setEnvelope(dumpStamp);
        dumpPort.writeStrict();

        yInfo()<<bDump.toString();

        return true;
    }
Esempio n. 24
0
 void update() {
     Bottle& status = port.prepare();
     mutex.wait();
     status.clear();
     addQueue(status);
     mutex.post();
     port.write();
 }
int main(int argc, char *argv[]) {
  Network yarp;
  //yarp::os::Node    *rosNode;
  BufferedPort<geometry_msgs_Point> xd_inputPort;
  xd_inputPort.setReadOnly();
  bool outputOk = xd_inputPort.open("/my_xd_in@/ros_tests");
  BufferedPort<geometry_msgs_Point> xd_outputPort;
  bool outputOk_3 = xd_outputPort.open("/gaze_point");

  BufferedPort<geometry_msgs_Point>  receiverBuff1Mux1;
  bool receiver1Mux1Ok = receiverBuff1Mux1.open("/my_other_port_in");
  BufferedPort<geometry_msgs_Point> outputPort;
  outputPort.setWriteOnly();
  bool outputOk_1 = outputPort.open("/my_x_out@/ros_tests");

 if(!outputOk_3)
  {
    printf("outputOk_3 failed to open\n");
    return -1;
  }

  if(!outputOk_1)
  {
    printf("outputOk_1 failed to open\n");
    return -1;
  }
  bool connectSuccess = false;
  while(!connectSuccess)
  {
    printf("\nTrying to connect to /iKinGazeCtrl/x:o ... ");
    connectSuccess = yarp.connect("/iKinGazeCtrl/x:o", receiverBuff1Mux1.getName());
    yarp::os::Time::delay(1);
  }

  connectSuccess = false;
  
  while(!connectSuccess)
  {
    printf("\nTrying to connect to /iKinGazeCtrl/xd:i ... ");
    connectSuccess = yarp.connect(xd_outputPort.getName(),"/iKinGazeCtrl/xd:i");
    yarp::os::Time::delay(1);
  }
  while(true){
    geometry_msgs_Point *reading1Mux1;
    reading1Mux1 = xd_inputPort.read();
    geometry_msgs_Point & out = xd_outputPort.prepare();
    out = *reading1Mux1;
    xd_outputPort.write();

    geometry_msgs_Point *reading1Mux;
    reading1Mux = receiverBuff1Mux1.read();

    geometry_msgs_Point & out_ = outputPort.prepare();
    out_ = *reading1Mux;
    outputPort.write();
  }
  return 0;
}
Esempio n. 26
0
 void saythis(std::string text)
 {
     Bottle& b=speechOut.prepare();
     b.clear();
     b.add(text.c_str());
     speechOut.writeStrict();
     log(now, text.c_str());
     return;
 }
Esempio n. 27
0
	void SamIter(void)
	{
		Bottle& B = bStarSend.prepare();		// prepare the bottle/port
		B.clear();
		B.addInt(i_ID);
		B.addDouble(i_X);
		B.addDouble(i_Y);
		B.addDouble(i_Angle);
		bStarSend.writeStrict();					// add stuff then send
	}
Esempio n. 28
0
 void log(double time, string strng)
 {
     stringstream logstream;
     logstream << setprecision(15) << time << " " << strng.c_str();
     cout << logstream.str() << endl;
     Bottle& b=logOut.prepare();
     b.clear();
     b.add(logstream.str().c_str());
     logOut.writeStrict();
 }
Esempio n. 29
0
 virtual void run() {
   Network::connect(name,"/reader","mcast");
   for (int i=0; i<50; i++) {
     Bottle& b = port.prepare();
     b.clear();
     b.addString(name);
     port.write();
     Time::delay(0.1);
   }
 }
Esempio n. 30
0
 void remove_cog(string text)
 {
     Bottle obj;
     obj.clear();
     obj.addString("delete"); // command to add/update an object
     obj.addString(text.c_str());
     port_out.prepare() = obj;
     port_out.write();
     Time::delay(0.1);
 }