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 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 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());
}
}
}
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;
}
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;
}
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;
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 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 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;
}
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 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 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 PortReaderBufferBase::read(ConnectionReader& connection)
{
if (connection.getReference()!=nullptr) {
//printf("REF %ld %d\n", (long int)connection.getReference(),
// connection.isValid());
return acceptObjectBase(connection.getReference(), nullptr);
}
if (mPriv->replier != nullptr) {
if (connection.getWriter()) {
return mPriv->replier->read(connection);
}
}
PortReaderPacket *reader = nullptr;
while (reader==nullptr) {
mPriv->stateSema.wait();
reader = mPriv->get();
if (reader && reader->getReader() == nullptr) {
PortReader *next = create();
yAssert(next != nullptr);
reader->setReader(next);
}
mPriv->stateSema.post();
if (reader==nullptr) {
mPriv->consumeSema.wait();
}
}
bool ok = false;
if (connection.isValid()) {
yAssert(reader->getReader()!=nullptr);
ok = reader->getReader()->read(connection);
reader->setEnvelope(connection.readEnvelope());
} else {
// this is a disconnection
// don't talk to this port ever again
mPriv->port = nullptr;
}
if (ok) {
mPriv->stateSema.wait();
bool pruned = false;
if (mPriv->ct>0 && mPriv->prune) {
PortReaderPacket *readerPacket =
mPriv->dropContent();
pruned = (readerPacket!=nullptr);
}
//mPriv->configure(reader, false, true);
mPriv->pool.addActivePacket(reader);
mPriv->ct++;
mPriv->stateSema.post();
if (!pruned) {
mPriv->contentSema.post();
}
//YARP_ERROR(Logger::get(), ">>>>>>>>>>>>>>>>> adding data");
} else {
mPriv->stateSema.wait();
mPriv->pool.addInactivePacket(reader);
mPriv->stateSema.post();
//YARP_ERROR(Logger::get(), ">>>>>>>>>>>>>>>>> skipping data");
// important to give reader a shot anyway, allowing proper closing
YARP_DEBUG(Logger::get(), "giving PortReaderBuffer chance to close");
mPriv->contentSema.post();
}
return ok;
}
bool fakeMotorDeviceServer::read(ConnectionReader &connection)
{
Bottle cmd,reply;
cmd.read(connection);
mutex.wait();
int codeIF=cmd.get(0).asVocab();
int codeMethod=cmd.get(1).asVocab();
if (codeIF==Vocab::encode("lim"))
{
if (codeMethod==Vocab::encode("get"))
{
int axis=cmd.get(2).asInt();
double min,max;
if (getLimits(axis,&min,&max))
{
reply.addVocab(Vocab::encode("ack"));
reply.addDouble(min);
reply.addDouble(max);
}
}
}
else if (codeIF==Vocab::encode("enc"))
{
if (codeMethod==Vocab::encode("axes"))
{
int ax;
if (getAxes(&ax))
{
reply.addVocab(Vocab::encode("ack"));
reply.addInt(ax);
}
}
}
else if (codeIF==Vocab::encode("vel"))
{
if (codeMethod==Vocab::encode("move"))
{
int axis=cmd.get(2).asInt();
double sp=cmd.get(3).asDouble();
if (velocityMove(axis,sp))
reply.addVocab(Vocab::encode("ack"));
}
else if (codeMethod==Vocab::encode("acc"))
{
int axis=cmd.get(2).asInt();
double acc=cmd.get(3).asDouble();
if (setRefAcceleration(axis,acc))
reply.addVocab(Vocab::encode("ack"));
}
else if (codeMethod==Vocab::encode("stop"))
{
int axis=cmd.get(2).asInt();
if (stop(axis))
reply.addVocab(Vocab::encode("ack"));
}
}
if (reply.size()==0)
reply.addVocab(Vocab::encode("nack"));
mutex.post();
if (ConnectionWriter *returnToSender=connection.getWriter())
reply.write(*returnToSender);
return true;
}
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;
}
