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()); std::string cat = nc.getCategory(); if (nc.getNestedName()!="") { //bool service = (cat.find("1") != std::string::npos); bool publish = (cat.find('+') != std::string::npos); bool subscribe = (cat.find('-') != std::string::npos); ContactStyle style; Contact c1(nc.getFullName()); Contact c2(std::string("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 YarpNameSpace::unregisterName(const std::string& name) { NestedContact nc; nc.fromString(name); std::string cat = nc.getCategory(); if (nc.getNestedName()!="") { //bool service = (cat.find("1") != std::string::npos); bool publish = (cat.find('+') != std::string::npos); bool subscribe = (cat.find('-') != std::string::npos); ContactStyle style; Contact c1(nc.getFullName()); Contact c2(std::string("topic:/") + nc.getNestedName()); if (subscribe) { disconnectPortFromTopic(c2, c1, style); } if (publish) { disconnectPortFromTopic(c1, c2, style); } } NameClient& nic = HELPER(this); return nic.unregisterName(name); }
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; }
bool Port::open(const Contact& contact, bool registerName, const char *fakeName) { Contact contact2 = contact; if (!NetworkBase::initialized()) { YARP_ERROR(Logger::get(), "YARP not initialized; create a yarp::os::Network object before using ports"); return false; } ConstString n = contact2.getName(); bool local = false; if (n == "" && contact2.getPort()<=0) { local = true; registerName = false; n = "..."; } NestedContact nc(n); if (nc.getNestedName()!="") { if (nc.getNodeName() == "") { Nodes& nodes = NameClient::getNameClient().getNodes(); nodes.requireActiveName(); ConstString node_name = nodes.getActiveName(); if (node_name!="") { n = n + node_name; } } } if (n!="" && n[0]!='/' && n[0]!='=' && n!="..." && n.substr(0,3)!="...") { if (fakeName==NULL) { Nodes& nodes = NameClient::getNameClient().getNodes(); ConstString node_name = nodes.getActiveName(); if (node_name!="") { // n = node_name + "=/" + n; n = "/" + n + "@" + node_name; } } } if (n!="" && n[0]!='/' && n[0]!='=' && n!="..." && n.substr(0,3)!="...") { if (fakeName==NULL) { YARP_SPRINTF1(Logger::get(),error, "Port name '%s' needs to start with a '/' character", n.c_str()); return false; } } if (n!="" && n!="..." && n[0]!='=' && n.substr(0,3)!="...") { if (fakeName==NULL) { ConstString prefix = NetworkBase::getEnvironment("YARP_PORT_PREFIX"); if (prefix!="") { n = prefix + n; contact2 = contact2.addName(n); } } } PortCoreAdapter *currentCore = &(HELPER(implementation)); if (currentCore!=NULL) { NestedContact nc; nc.fromString(n); if (nc.getNestedName()!="") { if (nc.getCategory()=="") { // we need to add in a category ConstString cat = ""; if (currentCore->commitToRead) { cat = "-"; } else if (currentCore->commitToWrite) { cat = "+"; } if (cat!="") { if (currentCore->commitToRpc) { cat += "1"; } contact2 = contact2.addName(nc.getNestedName() + cat + "@" + nc.getNodeName()); } else { YARP_SPRINTF1(Logger::get(),error, "Error: Port '%s' is not committed to being either an input or output port.", n.c_str()); YARP_SPRINTF0(Logger::get(),error, "YARP does not mind, but we are trying to register with a name server that does."); YARP_SPRINTF0(Logger::get(),error, "You can call Port::setWriteOnly() or Port::setReadOnly(), OR rename the port."); NestedContact nc2 = nc; nc2.setCategoryWrite(); YARP_SPRINTF1(Logger::get(),error, "For an output port, call it: %s (+ adds data)", nc2.toString().c_str()); nc2.setCategoryRead(); YARP_SPRINTF1(Logger::get(),error, "For an input port, call it: %s (- takes data)", nc2.toString().c_str()); return false; } } } } // Allow for open() to be called safely many times on the same Port if (currentCore->isOpened()) { PortCoreAdapter *newCore = new PortCoreAdapter(*this); YARP_ASSERT(newCore!=NULL); // copy state that should survive in a new open() if (currentCore->checkPortReader()!=NULL) { newCore->configReader(*(currentCore->checkPortReader())); } if (currentCore->checkReadCreator()!=NULL) { newCore->configReadCreator(*(currentCore->checkReadCreator())); } if (currentCore->checkWaitAfterSend()>=0) { newCore->configWaitAfterSend(currentCore->checkWaitAfterSend()); } close(); delete ((PortCoreAdapter*)implementation); implementation = newCore; } PortCoreAdapter& core = HELPER(implementation); core.openable(); if (NetworkBase::localNetworkAllocation()&&contact2.getPort()<=0) { YARP_DEBUG(Logger::get(),"local network allocation needed"); local = true; } bool success = true; Contact caddress = Contact::bySocket(contact2.getCarrier(), contact2.getHost(), contact2.getPort()) .addName(contact2.getName()); caddress.setNested(contact2.getNested()); Contact address = caddress; core.setReadHandler(core); if (contact2.getPort()>0 && contact2.getHost()!="") { registerName = false; } ConstString typ = getType().getName(); if (typ!="") { NestedContact nc; nc.fromString(contact2.getName()); nc.setTypeName(typ); contact2.setNested(nc); } if (registerName&&!local) { address = NetworkBase::registerContact(contact2); } core.setControlRegistration(registerName); success = (address.isValid()||local)&&(fakeName==NULL); if (success) { // create a node if needed Nodes& nodes = NameClient::getNameClient().getNodes(); nodes.prepare(address.getRegName().c_str()); } // If we are a service client, go ahead and connect if (success) { NestedContact nc; nc.fromString(address.getName()); if (nc.getNestedName()!="") { if (nc.getCategory() == "+1") { addOutput(nc.getNestedName()); } } } ConstString blame = "invalid address"; if (success) { success = core.listen(address,registerName); blame = "address conflict"; if (success) { success = core.start(); blame = "manager did not start"; } } if (success) { address = core.getAddress(); if (registerName&&local) { contact2 = contact2.addSocket(address.getCarrier(), address.getHost(), address.getPort()); contact2 = contact2.addName(address.getRegName().c_str()); Contact newName = NetworkBase::registerContact(contact2); core.resetPortName(newName.getName()); address = core.getAddress(); } else if (core.getAddress().getRegName()=="" && !registerName) { core.resetPortName(core.getAddress().addCarrier("").toURI()); core.setName(core.getAddress().getRegName()); } if (core.getVerbosity()>=1) { if (address.getRegName()=="") { YARP_INFO(Logger::get(), String("Anonymous port active at ") + address.toURI()); } else { YARP_INFO(Logger::get(), String("Port ") + address.getRegName() + " active at " + address.toURI()); } } } if (fakeName!=NULL) { success = core.manualStart(fakeName); blame = "unmanaged port failed to start"; } if (!success) { YARP_ERROR(Logger::get(), String("Port ") + (address.isValid()?(address.getRegName().c_str()):(contact2.getName().c_str())) + " failed to activate" + (address.isValid()?" at ":"") + (address.isValid()?address.toURI():String("")) + " (" + blame.c_str() + ")"); } if (success) { // create a node if needed Nodes& nodes = NameClient::getNameClient().getNodes(); nodes.add(*this); } return success; }
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(); }
bool isPublisher() { ConstString cat = nc.getCategory(); return (cat=="" || cat=="+"); }
bool isTopic() { ConstString cat = nc.getCategory(); return (cat=="" || cat=="+" || cat=="-"); }
bool isService() { ConstString cat = nc.getCategory(); return (cat=="" || cat=="+1" || cat=="-1"); }
bool isSubscriber() { ConstString cat = nc.getCategory(); return (cat=="" || cat=="-"); }
bool isServiceServer() { std::string cat = nc.getCategory(); return (cat=="" || cat=="-1"); }
bool isTopic() { std::string cat = nc.getCategory(); return (cat=="" || cat=="+" || cat=="-"); }
bool isPublisher() { std::string cat = nc.getCategory(); return (cat=="" || cat=="+"); }
bool isSubscriber() { std::string cat = nc.getCategory(); return (cat=="" || cat=="-"); }
bool isServiceClient() { std::string cat = nc.getCategory(); return (cat=="" || cat=="+1"); }