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"); } }
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; }