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;
}
