bool StoreBlob::readRaw(ConnectionReader& reader) {
int len = reader.expectInt();
String buf(YARP_STRINIT(len));
reader.expectBlock((const char *)buf.c_str(),len);
x = buf;
return true;
}
bool StoreBlob::readRaw(ConnectionReader& reader)
{
std::int32_t len = reader.expectInt32();
x.resize(len);
reader.expectBlock(const_cast<char*>(x.data()), len);
return true;
}
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 StoreString::readRaw(ConnectionReader& reader) {
int len = reader.expectInt();
String buf(YARP_STRINIT(len));
reader.expectBlock((const char *)buf.c_str(),len);
if (len>0) {
if (buf[len-1] == '\0') {
len--;
}
}
x = buf.substr(0,len);
return true;
}
virtual bool read(ConnectionReader& connection)
{
unsigned int incoming;
bool ok = connection.expectBlock((char*)&datum, sizeof(double));
ok=ok&&connection.expectBlock((char*)&incoming, sizeof(int));
//this resize only if needed
resize(incoming);
if (payloadSize>0)
ok=ok&&connection.expectBlock((char *)payload, payloadSize);
return ok;
}
bool yarp::os::Node::Helper::read(ConnectionReader& reader)
{
if (!reader.isValid()) {
return false;
}
NodeArgs na;
na.request.read(reader);
//printf("NODE API for %s received %s\n",
//name.c_str(),
//na.request.toString().c_str());
std::string key = na.request.get(0).asString();
na.args = na.request.tail().tail();
if (key=="getBusStats") {
getBusStats(na);
} else if (key=="getBusInfo") {
getBusInfo(na);
} else if (key=="getMasterUri") {
getMasterUri(na);
} else if (key=="shutdown") {
shutdown(na);
} else if (key=="getPid") {
getPid(na);
} else if (key=="getSubscriptions") {
getSubscriptions(na);
} else if (key=="getPublications") {
getPublications(na);
} else if (key=="paramUpdate") {
paramUpdate(na);
} else if (key=="publisherUpdate") {
publisherUpdate(na);
} else if (key=="requestTopic") {
requestTopic(na);
} else {
na.error("I have no idea what you are talking about");
}
if (na.should_drop) {
reader.requestDrop(); // ROS likes to close down.
}
if (reader.getWriter()) {
Bottle full;
full.addInt32(na.code);
full.addString(na.msg);
full.add(na.reply);
//printf("NODE %s <<< %s\n",
//name.c_str(),
//full.toString().c_str());
full.write(*reader.getWriter());
}
return true;
}
bool StoreString::readRaw(ConnectionReader& reader)
{
std::int32_t len = reader.expectInt32();
x.resize(len);
reader.expectBlock(const_cast<char*>(x.data()), len);
#ifndef YARP_NO_DEPRECATED // Since YARP 2.3.72
// This is needed for compatibility with versions of yarp before March 2015
if (len > 0) {
if (x[len - 1] == '\0') {
x.resize(len-1);
}
}
#endif // YARP_NO_DEPRECATED
return true;
}
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;
}
virtual bool read(ConnectionReader& connection) {
Bottle cmd, response;
cmd.read(connection);
printf("command received: %s\n", cmd.toString().c_str());
int code = cmd.get(0).asVocab();
switch (code) {
case VOCAB3('s','e','t'):
printf("set command received\n");
prop.put(cmd.get(1).asString().c_str(),cmd.get(2));
break;
case VOCAB3('g','e','t'):
printf("get command received\n");
response.addVocab(VOCAB2('i','s'));
response.add(cmd.get(1));
response.add(prop.find(cmd.get(1).asString().c_str()));
break;
}
if (response.size()>=1) {
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
response.write(*writer);
printf("response sent: %s\n", response.toString().c_str());
}
}
}
bool Storable::read(ConnectionReader& connection)
{
std::int32_t x = connection.expectInt32();
if (x != getCode()) {
return false;
}
return readRaw(connection);
}
WireReader::WireReader(ConnectionReader& reader) : reader(reader)
{
reader.convertTextMode();
state = &baseState;
flush_if_needed = false;
get_mode = false;
support_get_mode = false;
expecting = false;
}
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;
}
bool BottleImpl::fromBytes(ConnectionReader& reader) {
if (reader.isError()) return false;
int id = speciality;
YMSG(("READING, nest flag is %d\n", nested));
if (id==0) {
id = reader.expectInt();
YMSG(("READ subcode %d\n", id));
} else {
YMSG(("READ skipped subcode %d\n", speciality));
}
Storable *storable = Storable::createByCode(id);
if (storable==NULL) {
YARP_SPRINTF1(Logger::get(),error,"BottleImpl reader failed, unrecognized object code %d",id);
return false;
}
storable->readRaw(reader);
add(storable);
return true;
}
/**
* This helper function groups code to avoid duplication. It is not a member function of Image because
* there are problems with ImageNetworkHeader, anyhow the function is state-less and uses only parameters.
*/
inline bool readFromConnection(Image &dest, ImageNetworkHeader &header, ConnectionReader& connection)
{
dest.resize(header.width, header.height);
unsigned char *mem = dest.getRawImage();
int allocatedBytes = dest.getRawImageSize();
yAssert(mem != NULL);
//this check is redundant with assertion, I would remove it
if (dest.getRawImageSize() != header.imgSize) {
printf("There is a problem reading an image\n");
printf("incoming: width %d, height %d, code %d, quantum %d, size %d\n",
(int)header.width, (int)header.height,
(int)header.id,
(int)header.quantum, (int)header.imgSize);
printf("my space: width %d, height %d, code %d, quantum %d, size %d\n",
dest.width(), dest.height(), dest.getPixelCode(), dest.getQuantum(), allocatedBytes);
}
yAssert(allocatedBytes == header.imgSize);
bool ok = connection.expectBlock((char *)mem, allocatedBytes);
return (!connection.isError() && ok);
}
virtual bool read(ConnectionReader& connection) {
Bottle b,r;
bool ok = b.read(connection);
if (!ok) return false;
stackWrap->onTick();
r.addString("tack");
ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL)
r.write(*returnToSender);
return true;
}
bool PortCommand::read(ConnectionReader& reader) {
//ACE_DEBUG((LM_DEBUG,"PortCommand::readBlock"));
ch = '\0';
str = "";
if (!reader.isTextMode()) {
bool ok = reader.expectBlock(header.get(),header.length());
if (!ok) return false;
char *base = header.get();
if (base[4] == '~') {
ch = base[5];
if (ch=='\0') {
//str = reader.expectString(reader.getSize());
str = reader.expectText('\0').c_str();
if (reader.isError()) return false;
if (str.length()>0) {
ch = str[0];
}
}
} else {
return false;
}
} else {
str = reader.expectText().c_str();
if (reader.isError()) return false;
if (str.length()>0) {
ch = str[0];
}
}
return true;
}
bool Value::read(ConnectionReader& connection) {
if (proxy) {
delete proxy;
proxy = 0;
}
int x = connection.expectInt();
if ((x&0xffff) != x) return false;
if (!(x&BOTTLE_TAG_LIST)) return false;
int len = connection.expectInt();
if (len==0) return true;
if (len!=1) return false;
if (x==BOTTLE_TAG_LIST) {
x = connection.expectInt();
} else {
x &= ~BOTTLE_TAG_LIST;
}
if (connection.isError()) return false;
Storable *s = Storable::createByCode(x);
setProxy(s);
if (!proxy) return false;
return s->readRaw(connection);
}
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;
}
bool read(ConnectionReader& reader) override {
if (!reader.isValid()) {
return false;
}
receives++;
BottleImpl bot;
bot.read(reader);
if (expectation==std::string("")) {
report(1,"got unexpected input");
return false;
}
checkEqual(bot.toString(),expectation,"received bottle");
return true;
}
virtual bool read(ConnectionReader& connection) {
Bottle in, out;
in.read(connection);
// process data "in", prepare "out"
printf("Got message to reply to: %s\n", in.toString().c_str());
out.clear();
out.add("acknowledge");
out.append(in);
ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender!=NULL) {
out.write(*returnToSender);
}
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;
}
virtual bool read(ConnectionReader& connection) {
Bottle in, out;
bool ok = in.read(connection);
if (!ok) {
return false;
}
out.add(*active);
ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender!=NULL) {
out.write(*returnToSender);
}
return true;
}
bool CtrlThread::RpcPortCallback::read(ConnectionReader& connection)
{
Bottle input, reply;
bool ok = input.read(connection);
if (!ok)
return false;
else{
ctThread.parseRpcMessage(&input, &reply);
ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender!=NULL) {
reply.write(*returnToSender);
}
return true;
}
}
bool Stamp::read(ConnectionReader& connection) {
connection.convertTextMode();
int header = connection.expectInt();
if (header!=BOTTLE_TAG_LIST) { return false; }
int len = connection.expectInt();
if (len!=2) { return false; }
int code;
code = connection.expectInt();
if (code!=BOTTLE_TAG_INT) { return false; }
sequenceNumber = connection.expectInt();
code = connection.expectInt();
if (code!=BOTTLE_TAG_DOUBLE) { return false; }
timeStamp = connection.expectDouble();
if (connection.isError()) {
sequenceNumber = -1;
timeStamp = 0;
return false;
}
return true;
}
bool DataProcessor::read(ConnectionReader& connection) {
Bottle in, out;
bool ok = in.read(connection);
std::cout << "DataProcessor read bottle " << std::endl;
Stamp stamp;
// stamp.read(connection);
// std::cout << "DataProcessor read stamp!" << std::endl;
if (!ok) return false;
// process data "in", prepare "out"
out.append(in);
out.addList() = module->calculatePosition(in, stamp);
std::cout << "DataProcessor: " << in.toString() << std::endl;
// reply
ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender!=NULL) {
out.write(*returnToSender);
}
return true;
}
bool InferrenceRequestReader::read(ConnectionReader& connection) {
/** do rpcport */
printf("Waiting for an RPC message...\n");
Bottle cmd,response;
bool ok = cmd.read(connection);
if (!ok) return false;
ConnectionWriter *returnToSender = connection.getWriter();
// process data in b
printf("Got message: %s\n", cmd.toString().c_str());
std::string cmdstring = std::string(cmd.get(0).asString().c_str());
if (!cmdstring.compare("INFER")||!cmdstring.compare("infer")) {
std::string whichLearner = std::string(cmd.get(1).asString().c_str());
int whichLearnerN = parent->learnerInd(whichLearner);
if (whichLearnerN<0)whichLearnerN=parent->addLearner(whichLearner);
int numComponents=cmd.get(2).asInt();
Vector components(numComponents);
Vector input(numComponents);
for (int i=0;i<numComponents;i++) {
components[i]=cmd.get(3+i).asDouble();
}
for (int i=0;i<numComponents;i++) {
input[i]=cmd.get(numComponents+3+i).asDouble();
}
Vector output;
Matrix sigmaDump;
bool res = parent->gmmthreads[whichLearnerN]->doInference(input,components,output,sigmaDump);
if (!res) {
response.addString("NOTREADY");
printf("Sending reply: %s\n", response.toString().c_str());
response.write(*returnToSender);
return true;
} else {
response.addString("REGRESSED");
response.addInt(output.Size());
for (int i=0;i<output.Size();i++) {
response.addDouble(output[i]);
}
printf("Sending reply: %s\n", response.toString().c_str());
response.write(*returnToSender);
return true;
}
}else if (!cmdstring.compare("SAVEDATA")||!cmdstring.compare("savedata")) {
std::string whichLearner = std::string(cmd.get(1).asString().c_str());
int whichLearnerN = parent->learnerInd(whichLearner);
std::string saveName = std::string(cmd.get(2).asString().c_str());
parent->gmmthreads[whichLearnerN]->saveData(saveName);
response.addString("OK");
printf("Sending reply to save data request: %s\n", response.toString().c_str());
response.write(*returnToSender);
}else if (!cmdstring.compare("SAVEMODEL")||!cmdstring.compare("savemodel")) {
std::string whichLearner = std::string(cmd.get(1).asString().c_str());
int whichLearnerN = parent->learnerInd(whichLearner);
std::string saveName = std::string(cmd.get(2).asString().c_str());
parent->gmmthreads[whichLearnerN]->saveMap(saveName);
response.addString("OK");
printf("Sending reply to save model request: %s\n", response.toString().c_str());
response.write(*returnToSender);
} else if (!cmdstring.compare("RELEARN")||!cmdstring.compare("relearn")) {
std::string whichLearner = std::string(cmd.get(1).asString().c_str());
int whichLearnerN = parent->learnerInd(whichLearner);
std::string saveName = std::string(cmd.get(2).asString().c_str());
parent->gmmthreads[whichLearnerN]->forceRelearn();
response.addString("OK");
std::cout<<"Forced relearn for learner "<<whichLearner<<": sending response "<< response.toString().c_str()<<std::endl;
response.write(*returnToSender);
} else {
response.addString("UNKNOWNCOMMAND");
printf("Sending reply: %s\n", response.toString().c_str());
response.write(*returnToSender);
return true;
}
}
bool StoreVocab::readRaw(ConnectionReader& reader)
{
x = reader.expectInt32();
return true;
}
bool StoreFloat64::readRaw(ConnectionReader& reader)
{
x = reader.expectFloat64();
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;
}
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;
}
