bool RosLookup::lookupCore(const ConstString& name) {
Bottle req, reply;
req.addString("lookupNode");
req.addString("dummy_id");
req.addString(name);
rpc(getRosCoreAddress(), "xmlrpc", req, reply, verbose);
if (reply.get(0).asInt()!=1) {
fprintf(stderr, "Failure: %s\n", reply.toString().c_str());
return false;
}
ConstString url = reply.get(2).asString();
ConstString::size_type break1 = url.find("://",0);
if (break1==ConstString::npos) {
fprintf(stderr, "url not understood: %s\n", url.c_str());
return false;
}
ConstString::size_type break2 = url.find(":",break1+3);
if (break2==ConstString::npos) {
fprintf(stderr, "url not understood: %s\n", url.c_str());
return false;
}
ConstString::size_type break3 = url.find("/",break2+1);
if (break3==ConstString::npos) {
fprintf(stderr, "url not understood: %s\n", url.c_str());
return false;
}
hostname = url.substr(break1+3,break2-break1-3);
Value vportnum;
vportnum.fromString(url.substr(break2+1,break3-break2-1).c_str());
portnum = vportnum.asInt();
if (verbose) printf("%s\n", reply.toString().c_str());
valid = (portnum!=0);
rpc(getRosCoreAddress(), "xmlrpc", req, reply, verbose);
return valid;
}
Contact YarpNameSpace::registerContact(const Contact& contact) {
NameClient& nic = HELPER(this);
Contact address = nic.registerName(contact.getName().c_str(),
contact);
if (address.isValid()) {
NestedContact nc;
nc.fromString(address.getRegName().c_str());
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
//bool service = (cat.find("1") != ConstString::npos);
bool publish = (cat.find("+") != ConstString::npos);
bool subscribe = (cat.find("-") != ConstString::npos);
ContactStyle style;
Contact c1 = Contact::byName(nc.getFullName());
Contact c2 = Contact::byName(ConstString("topic:/") + nc.getNestedName());
if (subscribe) {
style.persistenceType = ContactStyle::END_WITH_TO_PORT;
connectPortToTopic(c2,c1,style);
}
if (publish) {
style.persistenceType = ContactStyle::END_WITH_FROM_PORT;
connectPortToTopic(c1,c2,style);
}
}
}
return address;
}
Contact NameServer::queryName(const ConstString& name) {
ConstString base = name;
ConstString pat = "";
if (name.find("/net=") == 0) {
size_t patStart = 5;
size_t patEnd = name.find('/',patStart);
if (patEnd>=patStart && patEnd!=ConstString::npos) {
pat = name.substr(patStart,patEnd-patStart);
base = name.substr(patEnd);
YARP_DEBUG(Logger::get(),ConstString("Special query form ") +
name + " (" + pat + "/" + base + ")");
}
}
NameRecord *rec = getNameRecord(base,false);
if (rec!=YARP_NULLPTR) {
if (pat!="") {
ConstString ip = rec->matchProp("ips",pat);
if (ip!="") {
SplitString sip(ip.c_str());
Contact c = rec->getAddress();
c.setHost(sip.get(0));
return c;
}
}
return rec->getAddress();
}
return Contact();
}
ConstString Protocol::getSenderSpecifier() {
Route r = getRoute();
// We pull the sender name from the route.
ConstString from = r.getFromName();
// But we need to add any qualifiers looking in the carrier
// name. Ideally, we wouldn't need to bundle that in with
// the sender name, but we do it for now in the name of
// backwards compatibility.
ConstString carrier = r.getCarrierName();
size_t start = carrier.find("+");
if (start!=String::npos) {
from += " (";
for (size_t i=start+1; i<(size_t)carrier.length(); i++) {
char ch = carrier[i];
if (ch=='+') {
from += ") (";
} else if (ch=='.') {
from += " ";
} else {
from += ch;
}
}
from += ")";
}
return from;
}
Contact MultiNameSpace::detectNameServer(bool useDetectedServer,
bool& scanNeeded,
bool& serverUsed) {
// This code looks like a placeholder that never got replaced.
// It is using a heuristic that namespaces with "/ros" in the
// name are ros namespaces. There's no need for guesswork like
// that anymore. Also, code duplication. Should spin this
// off into a proper plugin mechanism for namespaces.
ConstString name = NetworkBase::getNameServerName();
Contact fake, r;
if (name.find("/ros")!=ConstString::npos) {
RosNameSpace ns(fake);
r = ns.detectNameServer(useDetectedServer,scanNeeded,serverUsed);
if (r.isValid()&&useDetectedServer&&scanNeeded) {
HELPER(this).activate(true);
}
} else {
YarpNameSpace ns(fake);
r = ns.detectNameServer(useDetectedServer,scanNeeded,serverUsed);
if (r.isValid()&&useDetectedServer&&scanNeeded) {
HELPER(this).activate(true);
}
}
return r;
}
void FallbackNameClient::run() {
NameConfig nc;
Contact call = FallbackNameServer::getAddress();
DgramTwoWayStream send;
send.join(call, true);
listen.join(call, false);
if (!listen.isOk()) {
YARP_ERROR(Logger::get(), ConstString("Multicast not available"));
return;
}
ConstString msg = ConstString("NAME_SERVER query ") + nc.getNamespace();
send.beginPacket();
send.writeLine(msg.c_str(), (int)msg.length());
send.flush();
send.endPacket();
for (int i=0; i<5; i++) {
listen.beginPacket();
ConstString txt = listen.readLine();
listen.endPacket();
if (closed) return;
YARP_DEBUG(Logger::get(), ConstString("Fallback name client got ") + txt);
if (txt.find("registration ")==0) {
address = NameClient::extractAddress(txt);
YARP_INFO(Logger::get(), ConstString("Received address ") +
address.toURI());
return;
}
}
}
Bottle NameServer::ncmdList(int argc, char *argv[]) {
Bottle response;
ConstString prefix = "";
if (argc==1) {
prefix = STR(argv[0]);
}
response.addString("ports");
for (PLATFORM_MAP(ConstString,NameRecord)::iterator it = nameMap.begin(); it!=nameMap.end(); it++) {
NameRecord& rec = PLATFORM_MAP_ITERATOR_SECOND(it);
ConstString iname = rec.getAddress().getRegName();
if (iname.find(prefix)==0) {
if (iname==prefix || iname[prefix.length()]=='/' ||
prefix[prefix.length()-1]=='/') {
if (rec.getAddress().isValid()) {
response.addList() = botify(rec.getAddress());
}
}
}
}
return response;
}
Contact YarpNameSpace::unregisterName(const ConstString& name) {
NestedContact nc;
nc.fromString(name);
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
//bool service = (cat.find("1") != ConstString::npos);
bool publish = (cat.find("+") != ConstString::npos);
bool subscribe = (cat.find("-") != ConstString::npos);
ContactStyle style;
Contact c1 = Contact::byName(nc.getFullName());
Contact c2 = Contact::byName(ConstString("topic:/") + nc.getNestedName());
if (subscribe) {
disconnectPortFromTopic(c2,c1,style);
}
if (publish) {
disconnectPortFromTopic(c1,c2,style);
}
}
NameClient& nic = HELPER(this);
return nic.unregisterName(name);
}
ConstString RosNameSpace::toRosName(const ConstString& name) {
if (name.find(':')==ConstString::npos) return name;
ConstString result;
for (size_t i=0; i<name.length(); i++) {
if (name[i]!=':') {
result += name[i];
} else {
result += "__";
}
}
return result;
}
// helper method for "yarpdev" body
static void toDox(PolyDriver& dd, FILE *os) {
fprintf(os,"===============================================================\n");
fprintf(os,"== Options checked by device:\n== \n");
Bottle order = dd.getOptions();
for (int i=0; i<order.size(); i++) {
ConstString name = order.get(i).toString();
if (name=="wrapped"||(name.find(".wrapped")!=ConstString::npos)) {
continue;
}
ConstString desc = dd.getComment(name.c_str());
Value def = dd.getDefaultValue(name.c_str());
Value actual = dd.getValue(name.c_str());
ConstString out = "";
out += name;
if (!actual.isNull()) {
if (actual.toString()!="") {
out += "=";
if (actual.toString().length()<40) {
out += actual.toString().c_str();
} else {
out += "(value too long)";
}
}
}
if (!def.isNull()) {
if (def.toString()!="") {
out += " [";
if (def.toString().length()<40) {
out += def.toString().c_str();
} else {
out += "(value too long)";
}
out += "]";
}
}
if (desc!="") {
out += "\n ";
out += desc.c_str();
}
fprintf(os,"%s\n", out.c_str());
}
fprintf(os,"==\n");
fprintf(os,"===============================================================\n");
}
Contact MultiNameSpace::detectNameServer(bool useDetectedServer,
bool& scanNeeded,
bool& serverUsed) {
ConstString name = NetworkBase::getNameServerName();
Contact fake, r;
if (name.find("/ros")!=ConstString::npos) {
RosNameSpace ns(fake);
r = ns.detectNameServer(useDetectedServer,scanNeeded,serverUsed);
if (r.isValid()&&useDetectedServer&&scanNeeded) {
HELPER(this).activate(true);
}
} else {
YarpNameSpace ns(fake);
r = ns.detectNameServer(useDetectedServer,scanNeeded,serverUsed);
if (r.isValid()&&useDetectedServer&&scanNeeded) {
HELPER(this).activate(true);
}
}
return r;
}
Contact RosNameSpace::queryName(const ConstString& name) {
dbg_printf("ROSNameSpace queryName(%s)\n", name.c_str());
NestedContact nc(name);
ConstString full = name;
ConstString node = nc.getNodeName();
ConstString srv = nc.getNestedName();
ConstString cat = nc.getCategory();
bool is_service = false;
Bottle cmd,reply;
if (cat.find("-1")==ConstString::npos) {
cmd.addString("lookupNode");
cmd.addString("dummy_id");
cmd.addString(toRosNodeName(node));
NetworkBase::write(getNameServerContact(),
cmd, reply);
}
Contact contact;
if (reply.get(0).asInt()!=1) {
cmd.clear();
reply.clear();
cmd.addString("lookupService");
cmd.addString("dummy_id");
cmd.addString(toRosNodeName(node));
NetworkBase::write(getNameServerContact(),
cmd, reply);
is_service = true;
}
contact = Contact::fromString(reply.get(2).asString());
// unfortunate differences in labeling carriers
if (contact.getCarrier()=="rosrpc") {
contact = contact.addCarrier(ConstString("rossrv+service.") + name);
} else {
contact = contact.addCarrier("xmlrpc");
}
contact = contact.addName(name);
if (srv == "" || !is_service) return contact;
return Contact();
}
ConstString RosNameSpace::fromRosName(const ConstString& name) {
if (name.find("__")==ConstString::npos) return name;
// length is at least 2
ConstString result;
int ct = 0;
for (size_t i=0; i<name.length(); i++) {
if (name[i]!='_') {
if (ct) result += '_';
result += name[i];
ct = 0;
} else {
ct++;
if (ct==2) {
result += ':';
ct = 0;
}
}
}
if (ct) result += '_';
return result;
}
bool NameServiceOnTriples::cmdList(NameTripleState& act) {
if (!act.bottleMode) {
act.reply.addString("old");
} else {
act.reply.addString("ports");
}
lock();
Triple t;
t.setNameValue("port","*");
ConstString prefix = "";
if (act.cmd.size()>1) {
prefix = act.cmd.get(1).asString();
}
list<Triple> lst = act.mem.query(t, YARP_NULLPTR);
act.nestedMode = true;
for (list<Triple>::iterator it=lst.begin(); it!=lst.end(); it++) {
if (prefix=="") {
act.cmd.clear();
act.cmd.addString("query");
act.cmd.addString(it->value.c_str());
act.mem.reset();
cmdQuery(act,true);
} else {
ConstString iname = it->value.c_str();
if (iname.find(prefix)==0) {
if (iname==prefix || iname[prefix.length()]=='/' ||
prefix[prefix.length()-1]=='/') {
act.cmd.clear();
act.cmd.addString("query");
act.cmd.addString(iname);
act.mem.reset();
cmdQuery(act,true);
}
}
}
}
unlock();
return true;
}
int main(int argc, char *argv[]) {
Property p;
p.fromCommand(argc,argv);
// To make sure that the dev test are able to find all the devices
// compile by YARP, also the one compiled as dynamic plugins
// we add the build directory to the YARP_DATA_DIRS enviromental variable
// CMAKE_CURRENT_DIR is the define by the CMakeLists.txt tests file
ConstString dirs = CMAKE_BINARY_DIR +
yarp::os::Network::getDirectorySeparator() +
"share" +
yarp::os::Network::getDirectorySeparator() +
"yarp";
// Add TEST_DATA_DIR to YARP_DATA_DIRS in order to find the contexts used
// by the tests
dirs += yarp::os::Network::getPathSeparator() +
TEST_DATA_DIR;
// If set, append user YARP_DATA_DIRS
// FIXME check if this can be removed
Network::getEnvironment("YARP_DATA_DIRS");
if (!Network::getEnvironment("YARP_DATA_DIRS").empty()) {
dirs += yarp::os::Network::getPathSeparator() +
Network::getEnvironment("YARP_DATA_DIRS");
}
Network::setEnvironment("YARP_DATA_DIRS", dirs);
printf("YARP_DATA_DIRS=\"%s\"\n", Network::getEnvironment("YARP_DATA_DIRS").c_str());
// check where to put description of device
ConstString dest = "";
dest = p.check("doc",Value("")).toString();
ConstString fileName = p.check("file",Value("default.ini")).asString();
if (p.check("file")) {
p.fromConfigFile(fileName);
}
ConstString deviceName = p.check("device",Value("")).asString();
// if no device given, we should be operating a completely
// standard test harness like for libYARP_OS and libYARP_sig
if (deviceName=="") {
return harness_main(argc,argv);
}
// device name given - use special device testing procedure
#ifdef CHECK_FOR_LEAKS
mtrace();
#endif
int result = 0;
Network::init();
Network::setLocalMode(true);
ConstString seek = fileName.c_str();
ConstString exampleName = "";
int pos = seek.rfind('/');
if (pos==-1) {
pos = seek.rfind('\\');
}
if (pos==-1) {
pos = 0;
} else {
pos++;
}
int len = seek.find('.',pos);
if (len==-1) {
len = seek.length();
} else {
len -= pos;
}
exampleName = seek.substr(pos,len).c_str();
ConstString shortFileName = seek.substr(pos,seek.length()).c_str();
PolyDriver dd;
YARP_DEBUG(Logger::get(), "harness opening...");
bool ok = dd.open(p);
YARP_DEBUG(Logger::get(), "harness opened.");
result = ok?0:1;
ConstString wrapperName = "";
ConstString codeName = "";
DriverCreator *creator =
Drivers::factory().find(deviceName.c_str());
if (creator!=nullptr) {
wrapperName = creator->getWrapper();
codeName = creator->getCode();
}
if (dest!="") {
ConstString dest2 = dest.c_str();
if (result!=0) {
dest2 += ".fail";
}
FILE *fout = fopen(dest2.c_str(),"w");
if (fout==nullptr) {
printf("Problem writing to %s\n", dest2.c_str());
std::exit(1);
}
fprintf(fout,"/**\n");
fprintf(fout," * \\ingroup dev_examples\n");
fprintf(fout," *\n");
fprintf(fout," * \\defgroup %s Example for %s (%s)\n\n",
exampleName.c_str(),
deviceName.c_str(),
exampleName.c_str());
fprintf(fout, "Instantiates \\ref cmd_device_%s \"%s\" device implemented by %s.\n",
deviceName.c_str(), deviceName.c_str(), codeName.c_str());
fprintf(fout, "\\verbatim\n%s\\endverbatim\n",
getFile(fileName.c_str()).c_str());
fprintf(fout, "If this text is saved in a file called %s then the device can be created by doing:\n",
shortFileName.c_str());
fprintf(fout, "\\verbatim\nyarpdev --file %s\n\\endverbatim\n",
shortFileName.c_str());
fprintf(fout, "Of course, the configuration could be passed just as command line options, or as a yarp::os::Property object in a program:\n");
fprintf(fout, "\\code\n");
fprintf(fout, "Property p;\n");
fprintf(fout, "p.fromConfigFile(\"%s\");\n",
shortFileName.c_str());
fprintf(fout, "// of course you could construct the Property object on-the-fly\n");
fprintf(fout, "PolyDriver dev;\n");
fprintf(fout, "dev.open(p);\n");
fprintf(fout, "if (dev.isValid()) { /* use the device via view method */ }\n" );
fprintf(fout, "\\endcode\n");
fprintf(fout, "Here is a list of properties checked when starting up a device based on this configuration file. Note that which properties are checked can depend on whether other properties are present. In some cases properties can also vary between operating systems. So this is just an example\n\n");
toDox(dd,fout);
fprintf(fout, "\n\\sa %s\n\n",
codeName.c_str());
fprintf(fout, " */\n");
fclose(fout);
fout = nullptr;
}
if (ok) {
YARP_DEBUG(Logger::get(), "harness closing...");
dd.close();
YARP_DEBUG(Logger::get(), "harness closed.");
}
// just checking for crashes, not device creation
return result; //result;
}
int main(int argc, char *argv[]) {
Property p;
p.fromCommand(argc,argv);
// check where to put description of device
ConstString dest = "";
dest = p.check("doc",Value("")).toString();
ConstString fileName = p.check("file",Value("default.ini")).asString();
if (p.check("file")) {
p.fromConfigFile(fileName);
}
ConstString deviceName = p.check("device",Value("")).asString();
// if no device given, we should be operating a completely
// standard test harness like for libYARP_OS and libYARP_sig
if (deviceName=="") {
return harness_main(argc,argv);
}
// device name given - use special device testing procedure
#ifdef CHECK_FOR_LEAKS
mtrace();
#endif
int result = 0;
Network::init();
Network::setLocalMode(true);
ConstString seek = fileName.c_str();
ConstString exampleName = "";
int pos = seek.rfind('/');
if (pos==-1) {
pos = seek.rfind('\\');
}
if (pos==-1) {
pos = 0;
} else {
pos++;
}
int len = seek.find('.',pos);
if (len==-1) {
len = seek.length();
} else {
len -= pos;
}
exampleName = seek.substr(pos,len).c_str();
ConstString shortFileName = seek.substr(pos,seek.length()).c_str();
PolyDriver dd;
YARP_DEBUG(Logger::get(), "harness opening...");
bool ok = dd.open(p);
YARP_DEBUG(Logger::get(), "harness opened.");
result = ok?0:1;
ConstString wrapperName = "";
ConstString codeName = "";
DriverCreator *creator =
Drivers::factory().find(deviceName.c_str());
if (creator!=NULL) {
wrapperName = creator->getWrapper();
codeName = creator->getCode();
}
if (dest!="") {
ConstString dest2 = dest.c_str();
if (result!=0) {
dest2 += ".fail";
}
FILE *fout = fopen(dest2.c_str(),"w");
if (fout==NULL) {
printf("Problem writing to %s\n", dest2.c_str());
yarp::os::exit(1);
}
fprintf(fout,"/**\n");
fprintf(fout," * \\ingroup dev_examples\n");
fprintf(fout," *\n");
fprintf(fout," * \\defgroup %s Example for %s (%s)\n\n",
exampleName.c_str(),
deviceName.c_str(),
exampleName.c_str());
fprintf(fout, "Instantiates \\ref cmd_device_%s \"%s\" device implemented by %s.\n",
deviceName.c_str(), deviceName.c_str(), codeName.c_str());
fprintf(fout, "\\verbatim\n%s\\endverbatim\n",
getFile(fileName.c_str()).c_str());
fprintf(fout, "If this text is saved in a file called %s then the device can be created by doing:\n",
shortFileName.c_str());
fprintf(fout, "\\verbatim\nyarpdev --file %s\n\\endverbatim\n",
shortFileName.c_str());
fprintf(fout, "Of course, the configuration could be passed just as command line options, or as a yarp::os::Property object in a program:\n");
fprintf(fout, "\\code\n");
fprintf(fout, "Property p;\n");
fprintf(fout, "p.fromConfigFile(\"%s\");\n",
shortFileName.c_str());
fprintf(fout, "// of course you could construct the Property object on-the-fly\n");
fprintf(fout, "PolyDriver dev;\n");
fprintf(fout, "dev.open(p);\n");
fprintf(fout, "if (dev.isValid()) { /* use the device via view method */ }\n" );
fprintf(fout, "\\endcode\n");
fprintf(fout, "Here is a list of properties checked when starting up a device based on this configuration file. Note that which properties are checked can depend on whether other properties are present. In some cases properties can also vary between operating systems. So this is just an example\n\n");
toDox(dd,fout);
fprintf(fout, "\n\\sa %s\n\n",
codeName.c_str());
fprintf(fout, " */\n");
fclose(fout);
fout = NULL;
}
if (ok) {
YARP_DEBUG(Logger::get(), "harness closing...");
dd.close();
YARP_DEBUG(Logger::get(), "harness closed.");
}
// just checking for crashes, not device creation
return result; //result;
}
String NameConfig::getHostName(bool prefer_loopback) {
// try to pick a good host identifier
ConstString result = "127.0.0.1";
bool loopback = true;
// Pick an IPv4 address.
// Prefer non-local addresses, then shorter addresses.
// Avoid IPv6.
#ifdef YARP_HAS_ACE
ACE_INET_Addr *ips = NULL;
size_t count = 0;
if (ACE::get_ip_interfaces(count,ips)>=0) {
for (size_t i=0; i<count; i++) {
ConstString ip = ips[i].get_host_addr();
#else
int family, s;
char hostname[NI_MAXHOST]; hostname[NI_MAXHOST-1] = '\0';
ConstString ip;
struct ifaddrs *ifaddr, *ifa;
if (getifaddrs(&ifaddr) == -1) {
perror("getifaddrs in getIps");
exit(EXIT_FAILURE);
}
for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
if (ifa->ifa_addr == NULL) continue;
family = ifa->ifa_addr->sa_family;
if (family == AF_INET || family == AF_INET6) {
s = getnameinfo(ifa->ifa_addr,
(family == AF_INET) ? sizeof(struct sockaddr_in) :
sizeof(struct sockaddr_in6),
hostname, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
if (s != 0) {
printf("getnameinfo() failed: %s\n", gai_strerror(s));
exit(EXIT_FAILURE);
}
ip = ConstString(hostname);
#endif
YARP_DEBUG(Logger::get(), String("scanning network interface ") +
ip.c_str());
bool take = prefer_loopback;
if (ip.find(":")!=ConstString::npos) continue;
if (!take) {
if (result=="localhost") {
take = true; // can't be worse
}
if (loopback) {
take = true; // can't be worse
} else if (ip.length()<result.length()) {
take = true;
}
}
if (take) {
if (!prefer_loopback) result = ip;
loopback = false;
if (ip == "127.0.0.1" || ip == "127.1.0.1" ||
ip == "127.0.1.1") {
loopback = true;
}
#ifdef YARP_HAS_ACE
#ifdef YARP_ACE_ADDR_HAS_LOOPBACK_METHOD
loopback = ips[i].is_loopback();
#endif
#endif
if (prefer_loopback && loopback) {
result = ip;
}
}
}
#ifdef YARP_HAS_ACE
delete[] ips;
#endif
}
return result.c_str();
}
bool NameConfig::isLocalName(const String& name) {
bool result = false;
#ifdef YARP_HAS_ACE
ACE_INET_Addr *ips = NULL;
size_t count = 0;
if (ACE::get_ip_interfaces(count,ips)>=0) {
for (size_t i=0; i<count; i++) {
String ip = ips[i].get_host_addr();
if (ip==name) {
result = true;
break;
}
}
delete[] ips;
}
#else
/**
* If this does not work properly, use a more sophisticated way
* instead of just gethostname.
*/
char hostname[NI_MAXHOST]; hostname[NI_MAXHOST-1] = '\0';
gethostname(hostname, NI_MAXHOST);
if (strcmp(hostname, name.c_str()) == 0) result = true;
#endif
// just in case
if (name=="localhost"||name=="127.0.0.1") { result = true; }
return result;
}
yarp::os::Bottle NameConfig::getIpsAsBottle() {
yarp::os::Bottle result;
#ifdef YARP_HAS_ACE
ACE_INET_Addr *ips = NULL;
size_t count = 0;
if (ACE::get_ip_interfaces(count,ips)>=0) {
for (size_t i=0; i<count; i++) {
String ip = ips[i].get_host_addr();
result.addString(ip.c_str());
}
delete[] ips;
}
#else
int family, s;
char host[NI_MAXHOST];
struct ifaddrs *ifaddr, *ifa;
if (getifaddrs(&ifaddr) == -1) {
perror("getifaddrs in getIpsAsBottle");
exit(EXIT_FAILURE);
}
for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
if (ifa->ifa_addr == NULL) continue;
family = ifa->ifa_addr->sa_family;
if (family == AF_INET || family == AF_INET6) {
s = getnameinfo(ifa->ifa_addr,
(family == AF_INET) ? sizeof(struct sockaddr_in) :
sizeof(struct sockaddr_in6),
host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
if (s != 0) {
printf("getnameinfo() failed: %s\n", gai_strerror(s));
exit(EXIT_FAILURE);
}
result.addString(host);
}
}
#endif
return result;
}
String NameConfig::getIps() {
yarp::os::Bottle bot = getIpsAsBottle();
ConstString result = "";
for (int i=0; i<bot.size(); i++) {
ConstString ip = bot.get(i).asString();
if (i>0) {
result += " ";
}
result += ip;
}
return result.c_str();
}
void NameConfig::setAddress(const Address& address) {
this->address = address;
}
void NameConfig::setNamespace(const String& ns) {
space = ns;
}
String NameConfig::getNamespace(bool refresh) {
if (space==""||refresh) {
ConstString senv = NetworkBase::getEnvironment("YARP_NAMESPACE");
if (senv!="") {
spaces.fromString(senv.c_str());
} else {
String fname = getConfigFileName(YARP_CONFIG_NAMESPACE_FILENAME);
spaces.fromString(readConfig(fname).c_str());
}
space = spaces.get(0).asString().c_str();
if (space=="") {
space = "/root";
}
if (spaces.size()==0) {
spaces.addString("/root");
}
}
return space;
}
Bottle NameConfig::getNamespaces(bool refresh) {
getNamespace(refresh);
return spaces;
}
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;
}
Contact RosNameSpace::registerAdvanced(const Contact& contact, NameStore *store) {
dbg_printf("ROSNameSpace registerContact(%s / %s)\n",
contact.toString().c_str(),
contact.toURI().c_str());
NestedContact nc = contact.getNested();
if (nc.getNestedName()=="") {
nc.fromString(contact.getName());
}
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
if (cat == "-1") {
Bottle cmd, reply;
cmd.clear();
cmd.addString("registerService");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(toRosName(nc.getNestedName()));
Contact rosrpc = contact;
rosrpc.setCarrier("rosrpc");
cmd.addString(rosrpc.toURI());
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(contact.getName()));
}
cmd.addString(c.toURI());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
} else if (cat == "+" || cat== "-") {
Bottle cmd, reply;
cmd.clear();
cmd.addString((cat=="+")?"registerPublisher":"registerSubscriber");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(toRosName(nc.getNestedName()));
ConstString typ = nc.getTypeNameStar();
if (typ!="*"&&typ!="") {
// remap some basic native YARP types
if (typ=="yarp/image") {
typ = "sensor_msgs/Image";
}
if (typ.find('/')==ConstString::npos) {
typ = ConstString("yarp/") + typ;
}
}
cmd.addString(typ);
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(contact.getName()));
}
//Contact c = rosify(contact);
cmd.addString(c.toURI());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) {
fprintf(stderr, "ROS registration error: %s\n", reply.toString().c_str());
return Contact();
}
if (cat=="-") {
Bottle *publishers = reply.get(2).asList();
if (publishers && publishers->size()>=1) {
cmd.clear();
cmd.addString(contact.toURI());
cmd.addString("publisherUpdate");
cmd.addString("/yarp/RosNameSpace");
cmd.addString(toRosName(nc.getNestedName()));
cmd.addList() = *publishers;
mutex.wait();
bool need_start = false;
if (pending.size()==0) {
mutex.post();
stop();
need_start = true;
mutex.wait();
}
pending.addList() = cmd;
if (need_start) {
start();
}
mutex.post();
}
}
}
return contact;
}
// Remainder of method is supporting older /name+#/foo syntax
ConstString name = contact.getName();
size_t pub_idx = name.find("+#");
size_t sub_idx = name.find("-#");
ConstString node = "";
ConstString pub = "";
ConstString sub = "";
if (pub_idx!=ConstString::npos) {
node = name.substr(0, pub_idx);
pub = name.substr(pub_idx+2, name.length());
YARP_SPRINTF1(Logger::get(), debug, "Publish to %s", pub.c_str());
}
if (sub_idx!=ConstString::npos) {
node = name.substr(0, sub_idx);
sub = name.substr(sub_idx+2, name.length());
YARP_SPRINTF1(Logger::get(), debug, "Subscribe to %s", sub.c_str());
}
if (node=="") {
node = name;
}
YARP_SPRINTF4(Logger::get(), debug, "Name [%s] Node [%s] sub [%s] pub [%s]",
name.c_str(), node.c_str(), sub.c_str(), pub.c_str());
{
Bottle cmd, reply;
// for ROS, we fake port name registrations by
// registering them as nodes that publish to an arbitrary
// topic
cmd.clear();
cmd.addString("registerPublisher");
cmd.addString(toRosNodeName(node));
cmd.addString("/yarp/registration");
cmd.addString("*");
Contact c = rosify(contact);
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) {
return Contact();
}
}
if (pub!="") {
NetworkBase::connect(node, ConstString("topic:/") + pub);
}
if (sub!="") {
NetworkBase::connect(ConstString("topic:/") + sub, node);
}
Contact c = contact;
c.setName(node);
return c;
}
void fromCommand(int argc, char *argv[],bool wipe=true) {
String tag = "";
Bottle accum;
Bottle total;
bool qualified = false;
for (int i=0; i<argc; i++) {
String work = argv[i];
bool isTag = false;
if (work.length()>=2) {
if (work[0]=='-'&&work[1]=='-') {
work = work.substr(2,work.length()-2);
isTag = true;
if (work.find("::")!=String::npos) {
qualified = true;
}
}
}
if (isTag) {
if (tag!="") {
total.addList().copy(accum);
}
tag = work;
accum.clear();
} else {
if (work.find("\\")!=String::npos) {
// Specifically when reading from the command
// line, we will allow windows-style paths.
// Hence we have to break the "\" character
String buf = "";
for (unsigned int i=0; i<work.length(); i++) {
buf += work[i];
if (work[i]=='\\') {
buf += work[i];
}
}
work = buf;
}
}
accum.add(Value::makeValue(work.c_str()));
}
if (tag!="") {
total.addList().copy(accum);
}
if (!qualified) {
fromBottle(total,wipe);
return;
}
if (wipe) {
clear();
}
Bottle *cursor = NULL;
for (int i=0; i<total.size(); i++) {
cursor = NULL;
Bottle *term = total.get(i).asList();
if (!term) continue;
ConstString key = term->get(0).asString();
ConstString base = key;
while (key.length()>0) {
int at = key.find("::");
base = key;
if (at>=0) {
base = key.substr(0,at);
key = key.substr(at+2);
} else {
key = "";
}
Bottle& result = (cursor!=NULL)? (cursor->findGroup(base.c_str())) : owner.findGroup(base.c_str());
if (result.isNull()) {
if (!cursor) {
cursor = &putBottle((base).c_str());
} else {
cursor = &cursor->addList();
}
cursor->addString(base);
} else {
cursor = &result;
}
}
if (cursor) {
cursor->copy(*term);
cursor->get(0) = Value(base);
}
}
}
Contact RosNameSpace::unregisterAdvanced(const ConstString& name, NameStore *store) {
NestedContact nc;
nc.fromString(name);
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
if (cat == "-1") {
Nodes& nodes = NameClient::getNameClient().getNodes();
Contact c = nodes.getURI(name);
c.setCarrier("rosrpc");
c = rosify(c);
Bottle cmd, reply;
cmd.clear();
cmd.addString("unregisterService");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(nc.getNestedName());
cmd.addString(c.toURI());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
} else if (cat == "+" || cat== "-") {
Bottle cmd, reply;
cmd.clear();
cmd.addString((cat=="+")?"unregisterPublisher":"unregisterSubscriber");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(nc.getNestedName());
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(name));
}
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
}
return Contact();
}
// Remainder of method is supporting older /name+#/foo syntax
size_t pub_idx = name.find("+#");
size_t sub_idx = name.find("-#");
ConstString node = "";
ConstString pub = "";
ConstString sub = "";
if (pub_idx!=ConstString::npos) {
node = name.substr(0, pub_idx);
pub = name.substr(pub_idx+2, name.length());
}
if (sub_idx!=ConstString::npos) {
node = name.substr(0, sub_idx);
sub = name.substr(sub_idx+2, name.length());
}
if (node=="") {
node = name;
}
YARP_SPRINTF3(Logger::get(), debug, "Name [%s] sub [%s] pub [%s]\n",
name.c_str(), sub.c_str(), pub.c_str());
if (pub!="") {
NetworkBase::disconnect(name, ConstString("topic:/") + pub);
}
if (sub!="") {
NetworkBase::disconnect(ConstString("topic:/") + sub, name);
}
Contact contact = NetworkBase::queryName(name);
Bottle cmd, reply;
cmd.addString("unregisterPublisher");
cmd.addString(name);
cmd.addString("/yarp/registration");
Contact c("http", contact.getHost().c_str(), contact.getPort());
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
return Contact();
}