bool Protocol::write(SizedWriter& writer)
{
// End any current write.
writer.stopWrite();
// Skip if this connection is not active (e.g. when there are several
// logical mcast connections but only one write is actually needed).
if (!getConnection().isActive())
return false;
this->writer = &writer;
bool replied = false;
yAssert(delegate != nullptr);
getStreams().beginPacket(); // Message begins.
bool ok = delegate->write(*this, writer);
getStreams().endPacket(); // Message ends.
PortReader* reply = writer.getReplyHandler();
if (reply != nullptr) {
if (!delegate->supportReply()) {
// We are expected to get a reply, but cannot.
YARP_INFO(log, std::string("connection ") + getRoute().toString() + " does not support replies (try \"tcp\" or \"text_ack\")");
}
if (ok) {
// Read reply.
reader.reset(is(), &getStreams(), getRoute(), messageLen, delegate->isTextMode(), delegate->isBareMode());
replied = reply->read(reader);
}
}
expectAck(); // Expect acknowledgement (carrier-specific).
this->writer = nullptr;
return replied;
}
bool Bottle::write(PortReader& reader, bool textMode)
{
DummyConnector con;
con.setTextMode(textMode);
write(con.getWriter());
return reader.read(con.getReader());
}
bool ConnectionReader::readFromStream(PortReader& portable, InputStream& is)
{
StreamConnectionReader reader;
Route r;
reader.reset(is, nullptr, r, 0, false);
return portable.read(reader);
}
void checkType(PortReader& reader) {
if (!checkedType) {
if (!typ.isValid()) {
typ = reader.getReadType();
}
checkedType = true;
}
}
bool BufferedConnectionWriter::write(PortReader& obj)
{
DummyConnector con;
con.setTextMode(isTextMode());
if (!write(con.getWriter()))
return false;
return obj.read(con.getReader());
}
bool getEnvelope(PortReader& envelope)
{
if (prev==nullptr) {
return false;
}
StringInputStream sis;
sis.add(prev->envelope.c_str());
sis.add("\r\n");
StreamConnectionReader sbr;
Route route;
sbr.reset(sis, nullptr, route, 0, true);
return envelope.read(sbr);
}
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!="";
}
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& 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;
}
