bool RFModuleHelper::read(ConnectionReader& connection) {
Bottle cmd, response;
if (!cmd.read(connection)) { return false; }
printf("command received: %s\n", cmd.toString().c_str());
bool result = owner.safeRespond(cmd,response);
if (response.size()>=1) {
ConnectionWriter *writer = connection.getWriter();
if (writer!=0) {
if (response.get(0).toString()=="many" && writer->isTextMode()) {
for (int i=1; i<response.size(); i++) {
Value& v = response.get(i);
if (v.isList()) {
v.asList()->write(*writer);
} else {
Bottle b;
b.add(v);
b.write(*writer);
}
}
} else {
response.write(*writer);
}
//printf("response sent: %s\n", response.toString().c_str());
}
}
return result;
}
bool DeviceResponder::read(ConnectionReader& connection) {
Bottle cmd, response;
if (!cmd.read(connection)) { return false; }
//printf("command received: %s\n", cmd.toString().c_str());
respond(cmd,response);
if (response.size()>=1) {
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
if (response.get(0).toString()=="many"&&writer->isTextMode()) {
for (int i=1; i<response.size(); i++) {
Value& v = response.get(i);
if (v.isList()) {
v.asList()->write(*writer);
} else {
Bottle b;
b.add(v);
b.write(*writer);
}
}
} else {
response.write(*writer);
}
//printf("response sent: %s\n", response.toString().c_str());
}
} else {
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
response.clear();
response.addVocab(Vocab::encode("nak"));
response.write(*writer);
}
}
return true;
}
bool Value::write(ConnectionWriter& connection) {
if (!proxy) {
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(0);
return !connection.isError();
}
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(1);
return proxy->write(connection);
}
virtual bool write(ConnectionWriter& connection)
{
connection.appendBlock((char*)&datum, sizeof(double));
connection.appendBlock((char*)&payloadSize, sizeof(int));
if (payloadSize>0)
connection.appendBlock((char *)payload, payloadSize);
return !connection.isError();
}
bool BufferedConnectionWriter::write(ConnectionWriter& connection) const
{
stopWrite();
size_t i;
for (i = 0; i < header_used; i++) {
yarp::os::ManagedBytes& b = *(header[i]);
connection.appendBlock(b.get(), b.used());
}
for (i = 0; i < lst_used; i++) {
yarp::os::ManagedBytes& b = *(lst[i]);
connection.appendBlock(b.get(), b.used());
}
return !connection.isError();
}
bool yarp::os::SizedWriter::write(ConnectionWriter& connection) const
{
for (size_t i = 0; i < length(); i++) {
connection.appendBlock((char*)data(i), length(i));
}
return true;
}
bool Stamp::write(ConnectionWriter& connection) {
connection.appendInt(BOTTLE_TAG_LIST); // nested structure
connection.appendInt(2); // with two elements
connection.appendInt(BOTTLE_TAG_INT);
connection.appendInt(sequenceNumber);
connection.appendInt(BOTTLE_TAG_DOUBLE);
connection.appendDouble(timeStamp);
connection.convertTextMode();
return !connection.isError();
}
bool PortCommand::write(ConnectionWriter& writer) {
//ACE_DEBUG((LM_DEBUG,"PortCommand::writeBlock"));
//ACE_OS::printf("Writing port command, text mode %d\n", writer.isTextMode());
if (!writer.isTextMode()) {
int len = 0;
if (ch=='\0') {
len = (int)str.length()+1;
}
yAssert(header.length()==8);
char *base = header.get();
Bytes b(base,4);
NetType::netInt(len,b);
base[4] = '~';
base[5] = ch;
base[6] = 0;
base[7] = 1;
writer.appendBlock(header.bytes().get(),header.bytes().length());
if (ch=='\0') {
writer.appendBlock(str.c_str(),str.length()+1);
}
} else {
if (ch!='\0') {
char buf[] = "X";
buf[0] = ch;
writer.appendString(ConstString(buf).c_str(),'\n');
} else {
writer.appendString(str.c_str(),'\n');
}
}
return !writer.isError();
}
bool BottleImpl::write(ConnectionWriter& writer) {
// could simplify this if knew lengths of blocks up front
if (writer.isTextMode()) {
//writer.appendLine(toString());
writer.appendString(toString().c_str(),'\n');
} else {
#ifdef USE_YARP1_PREFIX
if (!nested) {
String name = "YARP2";
writer.appendInt(name.length()+1);
writer.appendString(name.c_str(),'\0');
}
#endif
synch();
/*
if (!nested) {
// No byte count any more, to facilitate nesting
//YMSG(("bottle byte count %d\n",byteCount()));
//writer.appendInt(byteCount()+sizeof(NetInt32));
writer.appendInt(StoreList::code + speciality);
}
*/
//writer.appendBlockCopy(Bytes((char*)getBytes(),byteCount()));
writer.appendBlock((char*)getBytes(),byteCount());
}
return !writer.isError();
}
bool StoreBlob::writeRaw(ConnectionWriter& writer) const
{
writer.appendInt32(static_cast<std::int32_t>(x.length()));
writer.appendBlock(x.c_str(), x.length());
return true;
}
bool StoreFloat64::writeRaw(ConnectionWriter& writer) const
{
writer.appendFloat64(x);
return true;
}
bool StoreVocab::writeRaw(ConnectionWriter& writer) const
{
writer.appendInt32(x);
return true;
}
bool StoreDouble::writeRaw(ConnectionWriter& writer) {
//writer.appendBlockCopy(Bytes((char*)&x,sizeof(x)));
NetFloat64 flt = x;
writer.appendBlock((char*)&flt,sizeof(flt));
return true;
}
virtual bool write(ConnectionWriter& connection) {
printf("Writing an integer\n");
connection.appendInt(val);
return true;
}
virtual bool write(ConnectionWriter& connection) {
ct = 0;
send.write(connection);
connection.setReplyHandler(*this);
return true;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~
// SERIALIZATION methods
//~~~~~~~~~~~~~~~~~~~~~~
bool dynContact::write(ConnectionWriter& connection) const{
// represent a dynContact as a list of 4 elements that are:
// - a list of 3 int, i.e. contactId, bodyPart, linkNumber
// - a list of 3 double, i.e. the CoP
// - a list of 3 double, i.e. the force
// - a list of 3 double, i.e. the moment
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(4);
// list of 3 int, i.e. contactId, bodyPart, linkNumber
connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_INT);
connection.appendInt(3);
connection.appendInt(contactId);
connection.appendInt(bodyPart); // left_arm, right_arm, ...
connection.appendInt(linkNumber);
// list of 3 double, i.e. the CoP
connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
connection.appendInt(3);
for(int i=0;i<3;i++) connection.appendDouble(CoP[i]);
// list of 3 double, i.e. the force
connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
connection.appendInt(3);
for(int i=0;i<3;i++) connection.appendDouble(F[i]);
// list of 3 double, i.e. the moment
connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
connection.appendInt(3);
for(int i=0;i<3;i++) connection.appendDouble(Mu[i]);
// if someone is foolish enough to connect in text mode,
// let them see something readable.
connection.convertTextMode();
return !connection.isError();
}
bool StoreBlob::writeRaw(ConnectionWriter& writer) {
writer.appendInt((int)x.length());
writer.appendBlock(x.c_str(),x.length());
return true;
}
bool StoreString::writeRaw(ConnectionWriter& writer) {
writer.appendInt((int)x.length()+1);
writer.appendBlock(x.c_str(),x.length()+1); // need \0
return true;
}
WireWriter::WireWriter(ConnectionWriter& writer) : writer(writer) {
get_mode = get_is_vocab = false;
need_ok = false;
writer.convertTextMode();
}
bool StoreInt16::writeRaw(ConnectionWriter& writer) const
{
writer.appendInt16(x);
return true;
}
bool StoreInt::writeRaw(ConnectionWriter& writer) {
writer.appendInt(x);
return true;
}
bool Storable::write(ConnectionWriter& connection) const
{
connection.appendInt32(getCode());
return writeRaw(connection);
}
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;
}
