Exemplo n.º 1
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.º 2
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");
        }
    }
Exemplo n.º 3
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.º 4
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");
 }
Exemplo 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;
}
Exemplo 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;

		}

	}
Exemplo n.º 8
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;
    }
Exemplo n.º 9
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.º 10
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.º 11
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.º 12
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.º 13
0
 bool updateModule()
 {
     ImageOf<PixelRgb> &img = portOut.prepare();
     global->readAndShow(img, showImages);
     portOut.write();
     return true;
 }
Exemplo n.º 14
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");
        }
    }
Exemplo n.º 15
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--;
    }
Exemplo n.º 16
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.º 17
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");
        }
    }
Exemplo n.º 18
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.º 19
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;
    }
Exemplo n.º 20
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;
}
Exemplo n.º 22
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);
   }
 }
Exemplo n.º 23
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);
 }
void YarpAdapter::writeVibrationDataToOutput(int* vibData) {
	Bottle& out = outPort.prepare();
	out.clear();

	// add vibration data to output
	for (int i = 0; i < GloveAdapter::NUM_OF_FINGERS; i++) {
		out.addInt(vibData[i]);
	}

	outPort.write();
}
    void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
    {
        ts.update();
        skinContactList &sCLout = outPort.prepare();
        sCLout.clear();

        sCLout = sCL;
        
        outPort.setEnvelope(ts);
        outPort.write();     
    }
Exemplo n.º 26
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.º 27
0
    void compute_and_send_command(int action_id)
    {
        //prepare the output command
        Bottle& bot = port_command_out.prepare();
        bot.clear();
        bot.addInt(actions.action_vector[action_id].counter);
        bot.addDouble(actions.action_vector[action_id].time);
        bot.addString(actions.action_vector[action_id].tag.c_str());
        //@@@ you can add stuff here...

        //send the output command
        port_command_out.write();
        if (!execute_joint_command(action_id))
        {
            yError("failed to execute command");
        }

        //quick reads the current position
        double encs[50];
        if (driver && driver->ienc_ll)
        {
            //driver->ienc_ll->getEncoders(encs);
            int nj; driver->ienc_ll->getAxes(&nj);
            for (int j = 0; j < nj; j++)
            {
                driver->ienc_ll->getEncoder(j, &encs[j]);
            }
        }
        else
        {
            //invalid driver
            yError("Critical error: invalid driver");
        }

        //send the joints angles on debug port
        double *ll = actions.action_vector[action_id].q_joints;
        Bottle& bot2 = this->port_command_joints.prepare();
        bot2.clear();
        bot2.addInt(actions.action_vector[action_id].counter);
        bot2.addDouble(actions.action_vector[action_id].time);
        int size = this->actions.action_vector[action_id].get_n_joints();
        bot2.addString("commands:");
        for (int ix=0;ix<size;ix++)
        {
            bot2.addDouble(ll[ix]);
        }
        bot2.addString("encoders:");
        for (int ix=0;ix<size;ix++)
        {
            bot2.addDouble(encs[ix]);
        }
        this->port_command_joints.write();
    }
Exemplo n.º 28
0
    bool read(ConnectionReader &connection)
    {
        Bottle data;
        data.read(connection);
        if ((data.size()>=2) && enabled)
        {
            Vector xa,oa;
            iarm->getPose(xa,oa);

            Vector xe,oe;
            if (eye=="left")
                igaze->getLeftEyePose(xe,oe);
            else
                igaze->getRightEyePose(xe,oe);

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

            Matrix He=axis2dcm(oe);
            He.setSubcol(xe,0,3);

            Matrix H=Prj*SE3inv(He)*Ha;
            Vector p(2);
            p[0]=data.get(0).asDouble();
            p[1]=data.get(1).asDouble();

            if (logPort.getOutputCount()>0)
            {
                Vector &log=logPort.prepare();

                log=p;
                for (int i=0; i<H.rows(); i++)
                    log=cat(log,H.getRow(i));
                for (int i=0; i<Prj.rows(); i++)
                    log=cat(log,Prj.getRow(i));
                for (int i=0; i<Ha.rows(); i++)
                    log=cat(log,Ha.getRow(i));
                for (int i=0; i<He.rows(); i++)
                    log=cat(log,He.getRow(i));

                logPort.write();
            }

            mutex.wait();
            solver.addItem(p,H);
            mutex.post();
        }

        return true;
    }
Exemplo n.º 29
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.º 30
0
    bool updateModule() {
        ImageOf<PixelRgb> *img = port.read();
        if (img==NULL) return false;;
        
        // 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 true;
    }