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; }
Contact NameClient::registerName(const String& name, const Contact& suggest) { String np = getNamePart(name); Bottle cmd; cmd.addString("register"); if (np!="") { cmd.addString(np.c_str()); } else { cmd.addString("..."); } ConstString prefix = NetworkBase::getEnvironment("YARP_IP"); NestedContact nc = suggest.getNested(); ConstString typ = nc.getTypeNameStar(); if (suggest.isValid()||prefix!=""||typ!="*") { if (suggest.getCarrier()!="") { cmd.addString(suggest.getCarrier().c_str()); } else { cmd.addString("..."); } if (suggest.getHost()!="") { cmd.addString(suggest.getHost().c_str()); } else { if (prefix!="") { Bottle ips = NameConfig::getIpsAsBottle(); for (int i=0; i<ips.size(); i++) { String ip = ips.get(i).asString().c_str(); if (ip.find(prefix)==0) { prefix = ip.c_str(); break; } } } cmd.addString((prefix!="")?prefix:"..."); } if (suggest.getPort()!=0) { cmd.addInt(suggest.getPort()); } else { cmd.addString("..."); } if (typ!="*") { cmd.addString(typ); } } Bottle reply; send(cmd,reply); Contact address = extractAddress(reply); if (address.isValid()) { String reg = address.getRegName(); /* // this never really got used cmd.fromString("set /port offers tcp text text_ack udp mcast shmem name_ser"); cmd.get(1) = Value(reg.c_str()); send(cmd,reply); // accept the same set of carriers cmd.get(2) = Value("accepts"); send(cmd,reply); */ cmd.clear(); cmd.addString("set"); cmd.addString(reg.c_str()); cmd.addString("ips"); cmd.append(NameConfig::getIpsAsBottle()); send(cmd,reply); cmd.clear(); cmd.addString("set"); cmd.addString(reg.c_str()); cmd.addString("process"); cmd.addInt(ACE_OS::getpid()); send(cmd,reply); } return address; }