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