bool test_surface_mesh() { printf("\n*** test_surface_mesh()\n"); SurfaceMesh mesh; Box3D bb; mesh.meshName = "testing"; bb.corners.push_back(PointXYZ(1,2,3)); SurfaceMeshWithBoundingBox obj(mesh,bb); Bottle bot; bot.read(obj); SurfaceMeshWithBoundingBox obj2; bot.write(obj2); Bottle bot2; bot2.read(obj2); printf("mesh copy: %s -> %s\n", bot.toString().c_str(), bot2.toString().c_str()); if (obj2.mesh.meshName!=obj.mesh.meshName) { printf("mesh name not copied correctly\n"); return false; } if (obj2.boundingBox.corners.size()!=obj.boundingBox.corners.size()) { printf("corners not copied correctly\n"); return false; } if (bot!=bot2) { printf("not copied correctly\n"); return false; } return true; }
//******************************************** bool read(ConnectionReader &connection) { Bottle input; input.read(connection); if (input.size()>=7) { string arm=input.get(0).asString().c_str(); transform(arm.begin(),arm.end(),arm.begin(),::tolower); mutex.lock(); map<string,Data>::iterator it=data.find(arm); if (it!=data.end()) { Data &d=it->second; d.point[0]=input.get(1).asDouble(); d.point[1]=input.get(2).asDouble(); d.point[2]=input.get(3).asDouble(); d.dir[0]=input.get(4).asDouble(); d.dir[1]=input.get(5).asDouble(); d.dir[2]=input.get(6).asDouble(); d.persistence=PPS_AVOIDANCE_PERSISTENCE; d.timeout=PPS_AVOIDANCE_TIMEOUT; } mutex.unlock(); } return true; }
virtual bool read(ConnectionReader& connection) { Bottle b; b.read(connection); // process data in b printf("Got %s\n", b.toString().c_str()); if(waitForFirst) { xKeep = b.get(0).asInt(); yKeep = b.get(1).asInt(); waitForFirst = false; } else { if((b.get(0).asInt()<xKeep)||(b.get(1).asInt()<yKeep)){ x = 0; y = 0; w = 0; h = 0; } else { x = xKeep; y = yKeep; w = b.get(0).asInt() - x; h = b.get(1).asInt() - y; } waitForFirst = true; } return true; }
virtual bool read(ConnectionReader& reader) override { go.wait(); mutex.lock(); bool ok = last.read(reader); mutex.unlock(); gone.post(); return ok; }
virtual bool read(ConnectionReader& reader) { go.wait(); mutex.wait(); bool ok = last.read(reader); mutex.post(); gone.post(); return ok; }
virtual bool read(ConnectionReader& connection) { Bottle receive; receive.read(connection); receive.addInt(5); ConnectionWriter *writer = connection.getWriter(); if (writer!=NULL) { receive.write(*writer); } return true; }
void testSerialCopy() { report(0,"test serialization by copy"); Value v(3.14); Bottle b; b.read(v); checkEqualish(b.get(0).asDouble(),3.14,"copy to bottle succeeded"); Bottle b2; b.write(b2); checkEqualish(b2.get(0).asDouble(),3.14,"copy from bottle succeeded"); }
void checkReadWrite() { report(0,"check read/write"); Value v(4.2); Bottle b; b.read(v); checkTrue(b.get(0).isFloat64(),"structure ok"); checkEqualish(b.get(0).asFloat64(),4.2,"value ok"); Value v2; b.write(v2); checkEqualish(v2.asFloat64(),4.2,"value reread ok"); }
bool read(ConnectionReader &connection) { Bottle data; data.read(connection); if (data.size()>=2) { LockGuard lg(mutex); cv::Point point(data.get(0).asInt(),data.get(1).asInt()); contour.push_back(point); } return true; }
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; }
virtual bool read(ConnectionReader& con) { Bottle b; if (!b.read(con)) { return false; } ConstString src = con.getRemoteContact().getName(); ConstString txt = b.get(0).asString(); printf("Reading [%s] from [%s]\n", txt.c_str(), src.c_str()); if (src!=txt) { printf("YIKES!!!\n"); exit(1); } return true; }
virtual bool read(ConnectionReader& connection) { Bottle receive; //printf("service provider reading data\n"); receive.read(connection); //printf("service provider read data\n"); receive.addInt(5); ConnectionWriter *writer = connection.getWriter(); if (writer!=NULL) { //printf("service provider replying\n"); receive.write(*writer); //printf("service provider replied\n"); } return true; }
bool StreamConnectionReader::convertTextMode() { if (isTextMode()) { if (!convertedTextMode) { Bottle bot; bot.read(*this); BufferedConnectionWriter writer; bot.write(writer); String s = writer.toString(); altStream.reset(s); in = &altStream; convertedTextMode = true; } } return true; }
//******************************************** bool read(ConnectionReader &connection) { Bottle in; in.read(connection); if (in.size()>=1) { Bottle input; input=*(in.get(0).asList()); if (input.size()>=7) { yDebug("[demoAvoidance] got command (%s)",input.toString().c_str()); string arm=iCub::skinDynLib::SkinPart_s[input.get(0).asInt()]; if (arm == "r_hand" || arm == "r_forearm") { arm = "right"; } else if (arm == "l_hand" || arm == "l_forearm") { arm = "left"; } transform(arm.begin(),arm.end(),arm.begin(),::tolower); mutex.lock(); map<string,Data>::iterator it=data.find(arm); if (it!=data.end()) { Data &d=it->second; d.point[0]=input.get(7).asDouble(); d.point[1]=input.get(8).asDouble(); d.point[2]=input.get(9).asDouble(); d.dir[0]=input.get(10).asDouble(); d.dir[1]=input.get(11).asDouble(); d.dir[2]=input.get(12).asDouble(); d.persistence=PPS_AVOIDANCE_PERSISTENCE; d.timeout=PPS_AVOIDANCE_TIMEOUT; } mutex.unlock(); } } return true; }
bool NetworkClock::read(ConnectionReader& reader) { Bottle bot; bool ok = bot.read(reader); if(closing) { _time = -1; return false; } if (!ok && !closing) { YARP_ERROR(Logger::get(), "Error reading clock port"); return false; } timeMutex.lock(); sec = bot.get(0).asInt32(); nsec = bot.get(1).asInt32(); _time = sec + (nsec*1e-9); initted = true; timeMutex.unlock(); listMutex.lock(); Waiters* waiters = static_cast<Waiters*>(pwaiters); Waiters::iterator waiter_i; waiter_i = waiters->begin(); while (waiter_i != waiters->end()) { if (waiter_i->first - _time < 1E-12 ) { Semaphore *waiterSemaphore = waiter_i->second; waiter_i = waiters->erase(waiter_i); if (waiterSemaphore) waiterSemaphore->post(); } else ++waiter_i; } listMutex.unlock(); return true; }
bool CalibModule::load() { string fileName=rf->findFile("calibrationFile").c_str(); if (fileName.empty()) { yWarning("calibration file not found"); return false; } Property data; data.fromConfigFile(fileName.c_str()); Bottle b; b.read(data); mutex.lock(); yInfo("loading experts from file: %s",fileName.c_str()); for (int i=0; i<b.size(); i++) factory(b.get(i)); mutex.unlock(); return true; }
virtual bool read(ConnectionReader& connection) { double now = Time::now(); Bottle b; bool ok = b.read(connection); if (ok) { mutex.wait(); ct++; if (ct>1) { double dt = now - lastTime; period.add(dt); } lastTime = now; printf("Period is %g +/- %g (%d)\n", period.mean(), period.deviation(), ct); mutex.post(); } return ok; }
bool yarp::os::impl::HttpCarrier::write(Protocol& proto, SizedWriter& writer) { DummyConnector con; con.setTextMode(true); for (size_t i=writer.headerLength(); i<writer.length(); i++) { con.getWriter().appendBlock(writer.data(i),writer.length(i)); } Bottle b; b.read(con.getReader()); ConstString body = b.find("web").toString(); if (body.length()!=0) { ConstString header; header += NetType::toHexString(body.length()).c_str(); header += "\r\n"; Bytes b2((char*)header.c_str(),header.length()); proto.os().write(b2); Bytes b3((char*)body.c_str(),body.length()); proto.os().write(b3); proto.os().write('\r'); proto.os().write('\n'); } else { ConstString txt = b.toString() + "\r\n"; ConstString header; header += NetType::toHexString(txt.length()).c_str(); header += "\r\n"; Bytes b2((char*)header.c_str(),header.length()); proto.os().write(b2); Bytes b3((char*)txt.c_str(),txt.length()); proto.os().write(b3); proto.os().write('\r'); proto.os().write('\n'); } proto.os().flush(); return proto.os().isOk(); }
bool YarpNameSpace::writeToNameServer(PortWriter& cmd, PortReader& reply, const ContactStyle& style) { Contact srv = getNameServerContact(); String cmd0 = "NAME_SERVER"; DummyConnector con0; cmd.write(con0.getWriter()); Bottle in; in.read(con0.getReader()); for (int i=0; i<in.size(); i++) { cmd0 += " "; cmd0 += in.get(i).toString().c_str(); } NameClient& nic = HELPER(this); String result = nic.send(cmd0); Bottle reply2; reply2.addString(result.c_str()); DummyConnector con; reply2.write(con.getWriter()); reply.read(con.getReader()); return result!=""; }
void checkPair() { report(0,"checking portable-pair serialization..."); // potential problem reported by Miguel Sarabia Del Castillo Matrix m; size_t rr = 10; size_t cc = 5; makeTestMatrix(m,rr,cc); double value = 3.14; yarp::os::PortablePair<yarp::sig::Matrix, yarp::os::Value> msg, msg2; msg.head = m; msg.body = yarp::os::Value(value); DummyConnector con; msg.write(con.getWriter()); ConnectionReader& reader = con.getReader(); msg2.read(reader); checkEqual(msg.head.rows(),msg2.head.rows(),"matrix row match"); checkEqual(msg.head.cols(),msg2.head.cols(),"matrix col match"); checkEqualish(msg.body.asFloat64(),msg2.body.asFloat64(),"value match"); Bottle bot; bot.read(msg); Bottle *bot1 = bot.get(0).asList(); Bottle *bot2 = bot.get(1).asList(); checkTrue(bot1!=nullptr&&bot2!=nullptr,"got head/body"); if (bot1==nullptr || bot2==nullptr) return; checkEqual((size_t) bot1->get(0).asInt32(),rr,"row count matches"); checkEqual((size_t) bot1->get(1).asInt32(),cc,"column count matches"); Bottle *lst = bot1->get(2).asList(); checkTrue(lst!=nullptr,"have data"); if (!lst) return; checkEqual(lst->size(),(rr*cc),"data length matches"); checkEqualish(bot2->get(0).asFloat64(),value,"value match"); }
void stack(PortWriter& msg, const ConstString& tag) { mutex.wait(); msgs.push_back(Bottle()); if (tag!="") { Bottle b; b.read(msg); Bottle& back = msgs.back(); back.clear(); back.addString(tag); back.append(b); } else { msgs.back().read(msg); } if (available_threads==0) { if (threads.size()<max_threads || max_threads == 0) { available_threads++; threads.push_back(new MessageStackThread(*this)); threads.back()->start(); } } available_threads--; mutex.post(); produce.post(); }
virtual bool read(ConnectionReader& reader) { YTRACE("NameServer::read start"); ConstString ref = "NAME_SERVER "; bool ok = true; ConstString msg = "?"; bool haveMessage = false; if (ok) { if (reader.isTextMode()) { msg = reader.expectText().c_str(); } else { // migrate to binary mode support, eventually optimize Bottle b; b.read(reader); msg = b.toString().c_str(); } haveMessage = (msg!=""); msg = ref + msg; } if (reader.isActive()&&haveMessage) { YARP_DEBUG(Logger::get(),ConstString("name server got message ") + msg); size_t index = msg.find("NAME_SERVER"); if (index==0) { Contact remote = reader.getRemoteContact(); YARP_DEBUG(Logger::get(), ConstString("name server receiving from ") + remote.toURI()); YARP_DEBUG(Logger::get(), ConstString("name server request is ") + msg); ConstString result = server->apply(msg,remote); ConnectionWriter *os = reader.getWriter(); if (os!=YARP_NULLPTR) { if (result=="") { result = ns_terminate(ConstString("unknown command ") + msg + "\n"); } // This change is just to make Microsoft Telnet happy ConstString tmp; for (unsigned int i=0; i<result.length(); i++) { if (result[i]=='\n') { tmp += '\r'; } tmp += result[i]; } tmp += '\r'; os->appendString(tmp.c_str(),'\n'); YARP_DEBUG(Logger::get(), ConstString("name server reply is ") + result); ConstString resultSparse = result; size_t end = resultSparse.find("\n*** end of message"); if (end!=ConstString::npos) { resultSparse[end] = '\0'; } YARP_INFO(Logger::get(),resultSparse); } } else { YARP_INFO(Logger::get(), ConstString("Name server ignoring unknown command: ")+msg); ConnectionWriter *os = reader.getWriter(); if (os!=YARP_NULLPTR) { // this result is necessary for YARP1 support os->appendString("???????????????????????????????????????",'\n'); //os->flush(); //os->close(); } } } YTRACE("NameServer::read stop"); return true; }
bool RosNameSpace::writeToNameServer(PortWriter& cmd, PortReader& reply, const ContactStyle& style) { DummyConnector con0; cmd.write(con0.getWriter()); Bottle in; in.read(con0.getReader()); ConstString key = in.get(0).asString(); ConstString arg1 = in.get(1).asString(); Bottle cmd2, cache; if (key=="query") { Contact c = queryName(arg1.c_str()); c.setName(""); Bottle reply2; reply2.addString(arg1.c_str()); reply2.addString(c.toString().c_str()); DummyConnector con; reply2.write(con.getWriter()); reply.read(con.getReader()); return true; } else if (key=="list") { cmd2.addString("getSystemState"); cmd2.addString("dummy_id"); if (!NetworkBase::write(getNameServerContact(), cmd2, cache, style)) { fprintf(stderr, "Failed to contact ROS server\n"); return false; } Bottle out; out.addVocab(Vocab::encode("many")); Bottle *parts = cache.get(2).asList(); Property nodes; Property topics; Property services; if (parts) { for (int i=0; i<3; i++) { Bottle *part = parts->get(i).asList(); if (!part) continue; for (int j=0; j<part->size(); j++) { Bottle *unit = part->get(j).asList(); if (!unit) continue; ConstString stem = unit->get(0).asString(); Bottle *links = unit->get(1).asList(); if (!links) continue; if (i<2) { topics.put(stem, 1); } else { services.put(stem, 1); } for (int j=0; j<links->size(); j++) { nodes.put(links->get(j).asString(), 1); } } } Property *props[3] = {&nodes, &topics, &services}; const char *title[3] = {"node", "topic", "service"}; for (int p=0; p<3; p++) { Bottle blist; blist.read(*props[p]); for (int i=0; i<blist.size(); i++) { ConstString name = blist.get(i).asList()->get(0).asString(); Bottle& info = out.addList(); info.addString(title[p]); info.addString(name); } } } out.write(reply); return true; } else { return false; } }
virtual bool read(ConnectionReader& connection) { receive.read(connection); ct++; return true; }
virtual bool read(ConnectionReader& reader) { // called by comms code readBlock.wait(); if (!reader.isValid()) { // termination stateMutex.wait(); if (readDelegate!=NULL) { readResult = readDelegate->read(reader); } stateMutex.post(); produce.post(); readBlock.post(); return false; } // wait for happy consumer - don't want to miss a packet if (!readBackground) { consume.wait(); } if (closed) { YARP_DEBUG(Logger::get(),"Port::read shutting down"); readBlock.post(); return false; } stateMutex.wait(); readResult = false; if (readDelegate!=NULL) { readResult = readDelegate->read(reader); } else { // read and ignore YARP_DEBUG(Logger::get(),"data received in Port, no reader for it"); Bottle b; b.read(reader); } if (!readBackground) { readDelegate = NULL; writeDelegate = NULL; } bool result = readResult; stateMutex.post(); if (!readBackground) { produce.post(); } if (result&&willReply) { consume.wait(); if (closed) { YARP_DEBUG(Logger::get(),"Port::read shutting down"); readBlock.post(); return false; } if (writeDelegate!=NULL) { stateMutex.wait(); ConnectionWriter *writer = reader.getWriter(); if (writer!=NULL) { result = readResult = writeDelegate->write(*writer); } stateMutex.post(); } if (dropDue) { reader.requestDrop(); } produce.post(); } readBlock.post(); return result; }
bool testSequence(char *seq, int len, const char *fmt, Bottle ref, bool testWrite = true) { printf("\n"); printf("================================================\n"); printf(" READ %s\n", fmt); Bytes b1(seq,len); WireTwiddler tt; tt.configure(fmt); printf(">>> %s\n", tt.toString().c_str()); Bottle bot; if (!tt.read(bot,b1)) { fprintf(stderr,"Read failed\n"); return 1; } if (bot!=ref) { printf("%s: read %s, expected %s\n", fmt, bot.toString().c_str(), ref.toString().c_str()); printf("MISMATCH\n"); exit(1); return false; } printf("[1] %s: read %s as expected\n", fmt, bot.toString().c_str()); StringInputStream sis; sis.add(b1); sis.add(b1); WireTwiddlerReader twiddled_input(sis,tt); Route route; StreamConnectionReader reader2; reader2.reset(twiddled_input,NULL,route,0,false); bot.clear(); twiddled_input.reset(); bot.read(reader2); if (bot!=ref) { printf("%s: read %s, expected %s\n", fmt, bot.toString().c_str(), ref.toString().c_str()); printf("MISMATCH\n"); exit(1); return false; } printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str()); StreamConnectionReader reader3; reader3.reset(twiddled_input,NULL,route,0,false); bot.clear(); twiddled_input.reset(); bot.read(reader3); if (bot!=ref) { printf("%s: read %s, expected %s\n", fmt, bot.toString().c_str(), ref.toString().c_str()); printf("MISMATCH\n"); exit(1); return false; } printf("[3] %s: read %s as expected\n", fmt, bot.toString().c_str()); if (testWrite) { printf("\n"); printf("================================================\n"); printf(" WRITE %s\n", fmt); ManagedBytes output; tt.write(ref,output); if (output.length()!=(size_t)len) { fprintf(stderr,"WRITE MISMATCH, length %d, expected %d\n", (int)output.length(), len); exit(1); return false; } for (size_t i=0; i<output.length(); i++) { if (output.get()[i] != seq[i]) { fprintf(stderr,"WRITE MISMATCH, at %d, have [%d:%c] expected [%d:%c]\n", (int)i, output.get()[i], output.get()[i], seq[i], seq[i]); exit(1); return false; } } printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str()); } return true; }
virtual bool read(ConnectionReader& con) { Bottle bot; bot.read(con); printf("Got %s\n", bot.toString().c_str()); return true; }
bool HapticDeviceWrapper::read(ConnectionReader &connection) { Bottle cmd; if (!cmd.read(connection)) return false; int tag=cmd.get(0).asVocab(); Bottle rep; if (device!=NULL) { LockGuard lg(mutex); if (tag==hapticdevice::set_transformation) { if (cmd.size()>=2) { if (Bottle *payload=cmd.get(1).asList()) { Matrix T(payload->get(0).asInt(), payload->get(1).asInt()); if (Bottle *vals=payload->get(2).asList()) { for (int r=0; r<T.rows(); r++) for (int c=0; c<T.cols(); c++) T(r,c)=vals->get(T.rows()*r+c).asDouble(); if (device->setTransformation(T)) rep.addVocab(hapticdevice::ack); else rep.addVocab(hapticdevice::nack); } } } } else if (tag==hapticdevice::get_transformation) { Matrix T; if (device->getTransformation(T)) { rep.addVocab(hapticdevice::ack); rep.addList().read(T); } else rep.addVocab(hapticdevice::nack); } else if (tag==hapticdevice::stop_feedback) { fdbck=0.0; device->stopFeedback(); applyFdbck=false; rep.addVocab(hapticdevice::ack); } else if (tag==hapticdevice::is_cartesian) { bool ret; if (device->isCartesianForceModeEnabled(ret)) { rep.addVocab(hapticdevice::ack); rep.addInt(ret?1:0); } else rep.addVocab(hapticdevice::nack); } else if (tag==hapticdevice::set_cartesian) { rep.addVocab(device->setCartesianForceMode()? hapticdevice::ack:hapticdevice::nack); } else if (tag==hapticdevice::set_joint) { rep.addVocab(device->setJointTorqueMode()? hapticdevice::ack:hapticdevice::nack); } else if (tag==hapticdevice::get_max) { Vector max; if (device->getMaxFeedback(max)) { rep.addVocab(hapticdevice::ack); rep.addList().read(max); } else rep.addVocab(hapticdevice::nack); } } if (rep.size()==0) rep.addVocab(hapticdevice::nack); ConnectionWriter *writer=connection.getWriter(); if (writer!=NULL) rep.write(*writer); return true; }
bool yarp::os::impl::HttpCarrier::reply(Protocol& proto, SizedWriter& writer) { DummyConnector con; con.setTextMode(true); for (size_t i=writer.headerLength(); i<writer.length(); i++) { con.getWriter().appendBlock(writer.data(i),writer.length(i)); } Bottle b; b.read(con.getReader()); ConstString mime = b.check("mime",Value("text/html")).asString(); ConstString body; bool using_json = false; if (stream!=NULL) { if (stream->useJson()) { mime = "text/json"; asJson(body,&b,stream->typeHint()); using_json = true; } } if (b.check("web")&&!using_json) { body = b.find("web").toString(); } if (b.check("stream")&&!using_json) { String header("HTTP/1.1 200 OK\r\nContent-Type: "); header += mime; header += "\r\n"; header += "Transfer-Encoding: chunked\r\n"; header += "\r\n"; int N = 2*1024; header += NetType::toHexString(body.length()+N); header += "\r\n"; Bytes b2((char*)header.c_str(),header.length()); proto.os().write(b2); // chrome etc won't render until enough chars are received. for (int i=0; i<N; i++) { proto.os().write(' '); } Bytes b3((char*)body.c_str(),body.length()); proto.os().write(b3); proto.os().write('\r'); proto.os().write('\n'); if (stream!=NULL) { stream->flip(); } return true; } if (stream!=NULL) { stream->finish(); } // Could check response codes, mime types here. if (body.length()!=0 || using_json) { ConstString mime = b.check("mime",Value("text/html")).asString(); String header("HTTP/1.1 200 OK\nContent-Type: "); header += mime; header += "\n"; header += "Access-Control-Allow-Origin: *\n"; header += "\n"; Bytes b2((char*)header.c_str(),header.length()); proto.os().write(b2); //body = b.toString(); Bytes b3((char*)body.c_str(),body.length()); proto.os().write(b3); } else { writer.write(proto.os()); } proto.os().flush(); return proto.os().isOk(); }