bool release() {
stateSema.wait();
PortWriter *cback = callback;
current = YARP_NULLPTR;
callback = YARP_NULLPTR;
stateSema.post();
if (cback!=YARP_NULLPTR) {
stateSema.wait();
outCt++;
stateSema.post();
cback->onCompletion();
}
return cback!=YARP_NULLPTR;
}
void resetExternal() {
if (writer!=NULL) {
writer->onCompletion();
writer = NULL;
}
external = NULL;
}
bool Bottle::read(const PortWriter& writer, bool textMode)
{
implementation->edit();
DummyConnector con;
con.setTextMode(textMode);
writer.write(con.getWriter());
return read(con.getReader());
}
void resetExternal()
{
if (writer!=nullptr) {
writer->onCompletion();
writer = nullptr;
}
external = nullptr;
}
/**
* write something to the port
*/
bool Port::write(PortWriter& writer, PortReader& reader,
PortWriter *callback) const {
PortCoreAdapter& core = HELPER(implementation);
if (core.isInterrupted()) return false;
bool result = false;
result = core.send(writer,&reader,callback);
if (!result) {
//YARP_DEBUG(Logger::get(), e.toString() + " <<<< Port::write saw this");
if (callback!=NULL) {
callback->onCompletion();
} else {
writer.onCompletion();
}
// leave result false
}
return result;
}
bool Port::write(const PortWriter& writer, const PortWriter *callback) const
{
PortCoreAdapter& core = IMPL();
if (core.isInterrupted()) return false;
core.alertOnWrite();
bool result = false;
//WritableAdapter adapter(writer);
result = core.send(writer, nullptr, callback);
//writer.onCompletion();
if (!result) {
//YARP_DEBUG(Logger::get(), e.toString() + " <<<< Port::write saw this");
if (callback!=nullptr) {
callback->onCompletion();
} else {
writer.onCompletion();
}
// leave result false
}
return result;
}
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;
}
}
bool NetworkBase::write(const Contact& contact,
PortWriter& cmd,
PortReader& reply,
const ContactStyle& style) {
if (!getNameSpace().serverAllocatesPortNumbers()) {
// switch to more up-to-date method
Port port;
port.setAdminMode(style.admin);
port.openFake("network_write");
Contact ec = contact;
if (style.carrier!="") {
ec.setCarrier(style.carrier);
}
if (!port.addOutput(ec)) {
if (!style.quiet) {
ACE_OS::fprintf(stderr, "Cannot make connection to '%s'\n",
ec.toString().c_str());
}
return false;
}
bool ok = port.write(cmd,reply);
return ok;
}
const char *connectionName = "admin";
ConstString name = contact.getName();
const char *targetName = name.c_str(); // use carefully!
Contact address = contact;
if (!address.isValid()) {
address = getNameSpace().queryName(targetName);
}
if (!address.isValid()) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"cannot find port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
address.setTimeout((float)style.timeout);
}
OutputProtocol *out = Carriers::connect(address);
if (out==YARP_NULLPTR) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"Cannot connect to port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
out->setTimeout(style.timeout);
}
Route r(connectionName,targetName,
(style.carrier!="")?style.carrier.c_str():"text_ack");
out->open(r);
PortCommand pc(0,style.admin?"a":"d");
BufferedConnectionWriter bw(out->getConnection().isTextMode(),
out->getConnection().isBareMode());
bool ok = true;
if (out->getConnection().canEscape()) {
ok = pc.write(bw);
}
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=YARP_NULLPTR) delete out;
return false;
}
ok = cmd.write(bw);
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=YARP_NULLPTR) delete out;
return false;
}
if (style.expectReply) {
bw.setReplyHandler(reply);
}
out->write(bw);
if (out!=YARP_NULLPTR) {
delete out;
out = YARP_NULLPTR;
}
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 ConnectionWriter::writeToStream(PortWriter& portable, OutputStream& os) {
BufferedConnectionWriter writer;
if (!portable.write(writer)) return false;
writer.write(os);
return os.isOk();
}
