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 test_surface_mesh() {
printf("\n*** test_surface_mesh()\n");
SurfaceMesh mesh;
Box3D bb;
mesh.meshName = "testing";
bb.corners.push_back(PointXYZ(1,2,3));
SurfaceMeshWithBoundingBox obj(mesh,bb);
Bottle bot;
bot.read(obj);
SurfaceMeshWithBoundingBox obj2;
bot.write(obj2);
Bottle bot2;
bot2.read(obj2);
printf("mesh copy: %s -> %s\n", bot.toString().c_str(),
bot2.toString().c_str());
if (obj2.mesh.meshName!=obj.mesh.meshName) {
printf("mesh name not copied correctly\n");
return false;
}
if (obj2.boundingBox.corners.size()!=obj.boundingBox.corners.size()) {
printf("corners not copied correctly\n");
return false;
}
if (bot!=bot2) {
printf("not copied correctly\n");
return false;
}
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;
}
void testStringWithNull() {
report(0,"test string with null");
char buf1[] = "hello world";
char buf2[] = "hello world";
buf2[5] = '\0';
ConstString str1(buf1,12);
ConstString str2(buf2,12);
checkEqual(str1.length(),12,"unmodified string length ok");
checkEqual(str2.length(),12,"modified string length ok");
ConstString str3(str2);
checkEqual(str3.length(),12,"copied string length ok");
Bottle bot;
bot.addString(str2);
checkEqual(bot.get(0).asString().length(),12,"bottled string asString() length ok");
checkEqual(bot.get(0).toString().length(),12,"bottled string toString() length ok");
Bottle bot2 = bot;
checkEqual(bot2.get(0).asString().length(),12,"bottled, copied string length ok");
Bottle bot3;
bot.write(bot3);
checkEqual(bot3.get(0).asString().length(),12,"bottled, serialized string length ok");
Bottle bot4;
bot4.fromString(bot.toString());
checkEqual(bot4.get(0).asString().length(),12,"bottled, text-serialized string length ok");
}
void testEmptyList() {
// based on a case submitted by Stephane Lallee; doesn't appear to
// fail here as he sees going from TCL to C++
report(0,"test empty list");
Bottle b;
b.fromString("appp plan-clean (\"<Plan>\" \"<Names>\" cover \"</Names>\" \"<isAtomic>\" 1 \"</isAtomic>\" \"<motorCommand>\" () \"</motorCommand>\" \"<Primitive>\" (cover 28988.470168 \"<args>\" \"7106\" \"7103\" \"</args>\" \"<argsRole>\" object1 arg2 object2 arg1 subject \"7107\" \"</argsRole>\" \"<preReqRelations>\" \"</preReqRelations>\" \"<preForRelations>\" \"</preForRelations>\" \"<postAddRelations>\" \"</postAddRelations>\" \"<postRemRelations>\" \"</postRemRelations>\" \"<addRelations>\" \"</addRelations>\" \"<remRelations>\" visible null arg2 \"</remRelations>\") \"</Primitive>\" \"<SubPlans>\" \"</SubPlans>\" \"</Plan>\")");
Bottle b2 = b;
checkEqual(b2.size(),3,"copy ok level 1");
Bottle *sub = b2.get(2).asList();
checkTrue(sub!=NULL,"list where list expected");
if (sub!=NULL) {
checkEqual(sub->size(),16,"copy ok level 2");
}
DummyConnector con;
con.setTextMode(false);
b.write(con.getWriter());
Bottle b3;
b3.read(con.getReader());
checkEqual(b3.size(),b.size(),"binary read/write ok");
sub = b3.get(2).asList();
checkTrue(sub!=NULL,"list where list expected");
if (sub!=NULL) {
checkEqual(sub->size(),16,"copy ok level 2");
}
}
void StatePort::onRead(Bottle &state)
{
if (client!=NULL)
{
LockGuard lg(client->mutex);
state.write(client->state);
getEnvelope(client->stamp);
}
}
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;
}
void testSerialCopy() {
report(0,"test serialization by copy");
Value v(3.14);
Bottle b;
b.read(v);
checkEqualish(b.get(0).asDouble(),3.14,"copy to bottle succeeded");
Bottle b2;
b.write(b2);
checkEqualish(b2.get(0).asDouble(),3.14,"copy from bottle succeeded");
}
void checkReadWrite() {
report(0,"check read/write");
Value v(4.2);
Bottle b;
b.read(v);
checkTrue(b.get(0).isFloat64(),"structure ok");
checkEqualish(b.get(0).asFloat64(),4.2,"value ok");
Value v2;
b.write(v2);
checkEqualish(v2.asFloat64(),4.2,"value reread 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;
}
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;
}
bool process() {
produce.wait();
if (!active) return false;
mutex.wait();
Bottle b = msgs.front();
msgs.pop_front();
mutex.post();
DummyConnector con;
b.write(con.getWriter());
owner.read(con.getReader());
mutex.wait();
available_threads++;
mutex.post();
return active;
}
bool StreamConnectionReader::convertTextMode() {
if (isTextMode()) {
if (!convertedTextMode) {
Bottle bot;
bot.read(*this);
BufferedConnectionWriter writer;
bot.write(writer);
String s = writer.toString();
altStream.reset(s);
in = &altStream;
convertedTextMode = true;
}
}
return true;
}
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 write(ConnectionWriter& connection) {
ct = 0;
send.write(connection);
connection.setReplyHandler(*this);
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;
}
