void update() {
if (nc.getTypeName()=="") {
if (!contactable) return;
Type typ = contactable->getType();
if (typ.isValid()) {
nc.setTypeName(typ.getName());
}
}
}
Contact getURI(const ConstString& name) {
Contact result;
mutex.lock();
NestedContact nc;
nc.fromString(name);
std::map<ConstString,Node *>::const_iterator it = by_name.find(nc.getNodeName());
if (it!=by_name.end()) {
result = it->second->query(nc.getNestedName());
}
mutex.unlock();
return result;
}
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;
}
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 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;
}
bool isTopic() {
ConstString cat = nc.getCategory();
return (cat=="" || cat=="+" || cat=="-");
}
bool isServiceClient()
{
std::string cat = nc.getCategory();
return (cat=="" || cat=="+1");
}
bool isSubscriber() {
ConstString cat = nc.getCategory();
return (cat=="" || cat=="-");
}
bool isService() {
ConstString cat = nc.getCategory();
return (cat=="" || cat=="+1" || cat=="-1");
}
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 isPublisher() {
ConstString cat = nc.getCategory();
return (cat=="" || cat=="+");
}
Contact NameServiceOnTriples::query(const yarp::os::ConstString& portName,
NameTripleState& act,
const yarp::os::ConstString& prefix,
bool nested) {
if (!nested) lock();
Triple t;
t.setNameValue("port",portName.c_str());
int result = act.mem.find(t, YARP_NULLPTR);
TripleContext context;
context.setRid(result);
if (result!=-1) {
ConstString host = "";
if (ConstString(prefix)!="") {
printf("LOOKING AT IPS FOR %s\n", prefix.c_str());
t.setNameValue("ips","*");
list<Triple> lst = act.mem.query(t,&context);
for (list<Triple>::iterator it=lst.begin();it!=lst.end();it++) {
printf("LOOKING AT IPS %s\n", it->value.c_str());
if (it->value.find(prefix)==0) {
host = it->value;
break;
}
}
}
if (host=="") {
t.setNameValue("host","*");
list<Triple> lst = act.mem.query(t,&context);
if (lst.size()>0) {
host = lst.begin()->value.c_str();
}
}
if (host=="") {
host = "localhost";
}
t.setNameValue("socket","*");
list<Triple> lst = act.mem.query(t,&context);
int sock = 10000;
if (lst.size()>0) {
sock = atoi(lst.begin()->value.c_str());
}
t.setNameValue("carrier","*");
ConstString carrier = "tcp";
lst = act.mem.query(t,&context);
if (lst.size()>0) {
carrier = lst.begin()->value.c_str();
}
t.setNameValue("type","*");
ConstString typ = "*";
lst = act.mem.query(t,&context);
if (lst.size()>0) {
typ = lst.begin()->value.c_str();
}
if (!nested) unlock();
Contact result = Contact(portName, carrier, host, sock);
if (typ!="" && typ!="*") {
NestedContact nc;
nc.fromString(result.getName());
nc.setTypeName(typ);
result.setNestedContact(nc);
}
return result;
}
if (!nested) unlock();
if (delegate && !nested) {
return delegate->queryName(portName);
}
return Contact();
}
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;
}
