void requestTopic(NodeArgs& na)
{
std::string topic = na.args.get(0).asString();
topic = fromRosName(topic);
std::vector<Contact> contacts = query(topic, "+");
if (contacts.size() < 1) {
na.fail("Cannot find topic");
return;
}
for (std::vector<Contact>::iterator it = contacts.begin(); it != contacts.end(); ++it) {
Contact &c = *it;
if (!c.isValid()) {
continue;
}
Value v;
Bottle* lst = v.asList();
lst->addString("TCPROS");
lst->addString(c.getHost());
lst->addInt32(c.getPort());
na.reply = v;
na.success();
return;
}
na.fail("Cannot find topic");
}
void publisherUpdate(NodeArgs& na)
{
std::string topic = fromRosName(na.args.get(0).asString());
std::vector<Contact> contacts = query(topic, "-");
if (contacts.size() < 1) {
na.fail("Cannot find topic");
return;
}
for (std::vector<Contact>::iterator it = contacts.begin(); it != contacts.end(); ++it) {
Contact &c = *it;
if (!c.isValid()) {
continue;
}
c.setName("");
// just pass the message along, YARP ports know what to do with it
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle reply;
if (!NetworkBase::write(c, na.request, reply, style)) {
continue;
}
na.fromExternal(reply);
}
}
void requestTopic(NodeArgs& na) {
ConstString topic = na.args.get(0).asString();
Contact c = lookup(topic);
if (!c.isValid()) {
na.fail("Cannot find topic");
return;
}
na.reply.addString("TCPROS");
na.reply.addString(c.getHost());
na.reply.addInt(c.getPort());
na.success();
}
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;
}
void publisherUpdate(NodeArgs& na) {
ConstString topic = na.args.get(0).asString();
Contact c = lookup(topic);
if (!c.isValid()) {
na.fail("Cannot find topic");
return;
}
c = c.addName("");
// just pass the message along, YARP ports know what to do with it
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle reply;
if (!NetworkBase::write(c,na.request,reply,style)) {
na.fail("Cannot communicate with local port");
return;
}
na.fromExternal(reply);
//printf("DONE with passing on publisherUpdate\n");
}
void getPublications(NodeArgs& na) {
mutex.lock();
for (std::map<ConstString,NodeItem>::iterator it = by_part_name.begin(); it != by_part_name.end(); it++) {
NodeItem& item = it->second;
if (!item.isPublisher()) continue;
item.update();
Bottle& lst = na.reply.addList();
lst.addString(item.nc.getNestedName());
lst.addString(item.nc.getTypeName());
}
mutex.unlock();
na.success();
}
void getBusInfo(NodeArgs& na)
{
unsigned int opaque_id = 1;
ROSReport report;
Value v;
Bottle* connections = v.asList();
mutex.lock();
for (std::multimap<std::string, NodeItem>::iterator it = by_part_name.begin(); it != by_part_name.end(); ++it) {
NodeItem& item = it->second;
if (!(item.isSubscriber() || item.isPublisher())) {
continue;
}
item.update();
item.contactable->getReport(report);
}
mutex.unlock();
for (std::multimap<std::string, std::string>::const_iterator it = report.outgoingURIs.begin(); it != report.outgoingURIs.end(); ++it) {
Bottle& lst = connections->addList();
lst.addInt32(opaque_id); // connectionId
lst.addString(it->second);
lst.addString("o");
lst.addString("TCPROS");
NestedContact nc(it->first);
lst.addString(toRosName(nc.getNestedName()));
opaque_id++;
}
for (std::multimap<std::string, std::string>::const_iterator it = report.incomingURIs.begin(); it != report.incomingURIs.end(); ++it) {
Bottle& lst = connections->addList();
lst.addInt32(opaque_id); // connectionId
lst.addString(it->second);
lst.addString("i");
lst.addString("TCPROS");
NestedContact nc(it->first);
lst.addString(toRosName(nc.getNestedName()));
opaque_id++;
}
if (connections->size() == 0) {
connections->addList(); // add empty list
}
na.reply = v;
na.success();
}
void getPublications(NodeArgs& na)
{
Value v;
Bottle* publications = v.asList();
mutex.lock();
for (std::multimap<std::string, NodeItem>::iterator it = by_part_name.begin(); it != by_part_name.end(); ++it) {
NodeItem& item = it->second;
if (!item.isPublisher()) {
continue;
}
item.update();
Bottle& lst = publications->addList();
lst.addString(toRosName(item.nc.getNestedName()));
lst.addString(item.nc.getTypeName());
}
mutex.unlock();
na.reply = v;
na.success();
}
void getPublications(NodeArgs& na)
{
Value v;
Bottle* publications = v.asList();
mutex.lock();
for (auto& it : by_part_name) {
NodeItem& item = it.second;
if (!item.isPublisher()) {
continue;
}
item.update();
Bottle& lst = publications->addList();
lst.addString(toRosName(item.nc.getNestedName()));
lst.addString(item.nc.getTypeName());
}
mutex.unlock();
na.reply = v;
na.success();
}
void getPid(NodeArgs& na) {
na.reply.addInt(ACE_OS::getpid());
na.success();
}
void getBusInfo(NodeArgs& na) {
na.reply.addList();
na.success();
}
void getBusStats(NodeArgs& na) {
na.reply.addList();
na.success();
}
void getPid(NodeArgs& na)
{
na.reply = Value(static_cast<int>(yarp::os::getpid()));
na.success();
}
void getMasterUri(NodeArgs& na)
{
na.reply = Value(NetworkBase::getEnvironment("ROS_MASTER_URI"));
na.success();
}
void getBusStats(NodeArgs& na)
{
na.reply = Value();
na.success();
}